bullet3/examples/SharedMemory/PhysicsClient.cpp

395 lines
11 KiB
C++
Raw Normal View History

#include "PhysicsClient.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "Bullet3Common/b3Logging.h"
#include "../Utils/b3ResourcePath.h"
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
2015-07-23 18:51:25 +00:00
#include "SharedMemoryBlock.h"
//copied from btMultiBodyLink.h
enum JointType
{
eRevoluteType = 0,
ePrismaticType = 1,
};
struct PhysicsClientSharedMemoryInternalData
{
SharedMemoryInterface* m_sharedMemory;
2015-07-23 18:51:25 +00:00
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
int m_counter;
bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer;
bool m_hasLastServerStatus;
PhysicsClientSharedMemoryInternalData()
:m_sharedMemory(0),
m_testBlock1(0),
m_counter(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(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()
{
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_data->m_sharedMemory;
delete m_data;
}
bool PhysicsClientSharedMemory::isConnected() const
{
return m_data->m_isConnected ;
}
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
bool allowCreation = true;
2015-07-23 18:51:25 +00:00
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)
{
2015-07-23 18:51:25 +00:00
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
{
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;
}
2015-07-23 18:51:25 +00:00
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{
bool hasStatus = false;
if (!m_data->m_testBlock1)
{
serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
return true;
}
if (!m_data->m_waitingForServer)
{
serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND;
return true;
}
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];
hasStatus = true;
serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command
switch (serverCmd.m_type)
{
case CMD_CLIENT_COMMAND_COMPLETED:
{
b3Printf("Server completed command");
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
m_data->m_serverLoadUrdfOK = true;
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)
{
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_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
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)
{
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)
{
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_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
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)
{
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())
{
b3Printf("Received robot description ok!\n");
} else
{
b3Warning("Robot description not received");
}
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
b3Printf("Server received desired state");
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
b3Printf("Server completed step simulation");
break;
}
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...\n");
m_data->m_serverLoadUrdfOK = false;
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
{
b3Printf("Server received bullet data stream OK\n");
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
{
b3Printf("Server failed receiving bullet data stream\n");
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
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;
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);
}
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);
}
b3Printf(msg);
b3Printf("\n");
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;
}
} else
{
b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
m_data->m_testBlock1->m_numProcessedServerCommands);
}
return hasStatus;
}
bool PhysicsClientSharedMemory::canSubmitCommand() const
{
return (m_data->m_isConnected && !m_data->m_waitingForServer);
}
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)
{
m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true;
return true;
}
return false;
}