Merge pull request #966 from erwincoumans/master

add tiny clsocket cross-platform TCP  library, update pybullet PDF doc
This commit is contained in:
erwincoumans
2017-02-20 21:17:22 -08:00
committed by GitHub
34 changed files with 5004 additions and 18 deletions

View File

@@ -558,12 +558,18 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
} else
{
state->m_jointPosition=0;
state->m_jointVelocity=0;
}
for (int ii(0); ii < 6; ++ii)
{
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
}
return 0;

View File

@@ -0,0 +1,255 @@
#include "PhysicsClientTCP.h"
#include "ActiveSocket.h"
#include <stdio.h>
#include <string.h>
#include "../Utils/b3Clock.h"
#include "PhysicsClient.h"
//#include "LinearMath/btVector3.h"
#include "SharedMemoryCommands.h"
#include <string>
#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
unsigned int b3DeserializeInt2(const unsigned char* input)
{
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
return tmp;
}
bool gVerboseNetworkMessagesClient2 = false;
struct TcpNetworkedInternalData
{
/*
ENetHost* m_client;
ENetAddress m_address;
ENetPeer* m_peer;
ENetEvent m_event;
*/
CActiveSocket m_tcpSocket;
bool m_isConnected;
TcpNetworkedInternalData* m_tcpInternalData;
SharedMemoryCommand m_clientCmd;
bool m_hasCommand;
SharedMemoryStatus m_lastStatus;
b3AlignedObjectArray<char> m_stream;
std::string m_hostName;
int m_port;
b3AlignedObjectArray<unsigned char> m_tempBuffer;
TcpNetworkedInternalData()
:
m_isConnected(false),
m_hasCommand(false)
{
}
bool connectTCP()
{
if (m_isConnected)
return true;
m_tcpSocket.Initialize();
m_isConnected = m_tcpSocket.Open(m_hostName.c_str(),m_port);
return m_isConnected;
}
bool checkData()
{
bool hasStatus = false;
//int serviceResult = enet_host_service(m_client, &m_event, 0);
int maxLen = 4 + sizeof(SharedMemoryStatus)+SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
int rBytes = m_tcpSocket.Receive(maxLen);
if (rBytes<=0)
return false;
//append to tmp buffer
//recBytes
unsigned char* d2 = (unsigned char*)m_tcpSocket.GetData();
int curSize = m_tempBuffer.size();
m_tempBuffer.resize(curSize+rBytes);
for (int i=0;i<rBytes;i++)
{
m_tempBuffer[curSize+i] = d2[i];
}
int packetSizeInBytes = -1;
if (m_tempBuffer.size()>=4)
{
packetSizeInBytes = b3DeserializeInt2(&m_tempBuffer[0]);
}
if (m_tempBuffer.size() == packetSizeInBytes)
{
unsigned char* data = &m_tempBuffer[0];
if (gVerboseNetworkMessagesClient2)
{
printf("A packet of length %d bytes received\n", m_tempBuffer.size());
}
hasStatus = true;
SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&data[4];
if (statPtr->m_type == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
{
SharedMemoryStatus dummy;
dummy.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_lastStatus = dummy;
m_stream.resize(0);
}
else
{
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
m_stream.resize(numStreamBytes);
for (int i = 0; i < numStreamBytes; i++)
{
m_stream[i] = data[i + streamOffsetInBytes];
}
}
m_tempBuffer.clear();
}
return hasStatus;
}
};
TcpNetworkedPhysicsProcessor::TcpNetworkedPhysicsProcessor(const char* hostName, int port)
{
m_data = new TcpNetworkedInternalData;
if (hostName)
{
m_data->m_hostName = hostName;
}
m_data->m_port = port;
}
TcpNetworkedPhysicsProcessor::~TcpNetworkedPhysicsProcessor()
{
disconnect();
delete m_data;
}
bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if(gVerboseNetworkMessagesClient2)
{
printf("PhysicsClientTCP::processCommand\n");
}
{
int sz = 0;
unsigned char* data = 0;
m_data->m_tempBuffer.clear();
if (clientCmd.m_type == CMD_STEP_FORWARD_SIMULATION)
{
sz = sizeof(int);
data = (unsigned char*) &clientCmd.m_type;
}
else
{
sz = sizeof(SharedMemoryCommand);
data = (unsigned char*)&clientCmd;
}
int res;
m_data->m_tcpSocket.Send((const uint8 *)data,sz);
}
return false;
}
bool TcpNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = m_data->checkData();
if (hasStatus)
{
if (gVerboseNetworkMessagesClient2)
{
printf("TcpNetworkedPhysicsProcessor::receiveStatus\n");
}
serverStatusOut = m_data->m_lastStatus;
int numStreamBytes = m_data->m_stream.size();
if (numStreamBytes < bufferSizeInBytes)
{
for (int i = 0; i < numStreamBytes; i++)
{
bufferServerToClient[i] = m_data->m_stream[i];
}
}
else
{
printf("Error: steam buffer overflow\n");
}
}
return hasStatus;
}
void TcpNetworkedPhysicsProcessor::renderScene()
{
}
void TcpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
void TcpNetworkedPhysicsProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
}
bool TcpNetworkedPhysicsProcessor::isConnected() const
{
return m_data->m_isConnected;
}
bool TcpNetworkedPhysicsProcessor::connect()
{
bool isConnected = m_data->connectTCP();
return isConnected;
}
void TcpNetworkedPhysicsProcessor::disconnect()
{
m_data->m_tcpSocket.Close();
m_data->m_isConnected = false;
}

View File

@@ -0,0 +1,37 @@
#ifndef PHYSICS_CLIENT_TCP_H
#define PHYSICS_CLIENT_TCP_H
#include "PhysicsDirect.h"
#include "PhysicsServerCommandProcessor.h"
class TcpNetworkedPhysicsProcessor : public PhysicsCommandProcessorInterface
{
struct TcpNetworkedInternalData* m_data;
public:
TcpNetworkedPhysicsProcessor(const char* hostName, int port);
virtual ~TcpNetworkedPhysicsProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
};
#endif //PHYSICS_CLIENT_TCP_H

View File

@@ -0,0 +1,29 @@
#include "PhysicsClientTCP_C_API.h"
#include "PhysicsClientTCP.h"
#include "PhysicsDirect.h"
#include <stdio.h>
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
{
TcpNetworkedPhysicsProcessor* tcp = new TcpNetworkedPhysicsProcessor(hostName, port);
PhysicsDirect* direct = new PhysicsDirect(tcp, true);
bool connected;
connected = direct->connect();
if (connected)
{
printf("b3ConnectPhysicsTCP connected successfully.\n");
}
else
{
printf("b3ConnectPhysicsTCP connection failed.\n");
}
return (b3PhysicsClientHandle)direct;
}

View File

@@ -0,0 +1,19 @@
#ifndef PHYSICS_CLIENT_TCP_C_API_H
#define PHYSICS_CLIENT_TCP_C_API_H
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
///send physics commands using TCP networking
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_TCP_C_API_H

View File

@@ -367,3 +367,5 @@ end
include "udp"
include "tcp"

View File

@@ -0,0 +1,265 @@
#include "PassiveSocket.h" // Include header for active socket object definition
#include <stdio.h>
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#ifdef NO_SHARED_MEMORY
#include "PhysicsServerCommandProcessor.h"
typedef PhysicsServerCommandProcessor MyCommandProcessor;
#else
#include "SharedMemoryCommandProcessor.h"
typedef SharedMemoryCommandProcessor MyCommandProcessor;
#endif //NO_SHARED_MEMORY
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "PhysicsServerCommandProcessor.h"
#include "../Utils/b3Clock.h"
bool gVerboseNetworkMessagesServer = true;
void MySerializeInt(unsigned int sz, unsigned char* output)
{
unsigned int tmp = sz;
output[0] = tmp & 255;
tmp = tmp >> 8;
output[1] = tmp & 255;
tmp = tmp >> 8;
output[2] = tmp & 255;
tmp = tmp >> 8;
output[3] = tmp & 255;
}
int main(int argc, char *argv[])
{
b3CommandLineArgs parseArgs(argc,argv);
b3Clock clock;
double timeOutInSeconds = 10;
DummyGUIHelper guiHelper;
MyCommandProcessor* sm = new MyCommandProcessor;
sm->setGuiHelper(&guiHelper);
int port = 6667;
if (parseArgs.GetCmdLineArgument("port",port))
{
printf("Using TCP port %d\n", port);
}
gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
#ifndef NO_SHARED_MEMORY
int key = 0;
if (parseArgs.GetCmdLineArgument("sharedMemoryKey",key))
{
sm->setSharedMemoryKey(key);
}
#endif//NO_SHARED_MEMORY
bool isPhysicsClientConnected = sm->connect();
bool exitRequested = false;
if (isPhysicsClientConnected)
{
CPassiveSocket socket;
CActiveSocket *pClient = NULL;
//--------------------------------------------------------------------------
// Initialize our socket object
//--------------------------------------------------------------------------
socket.Initialize();
socket.Listen("localhost", port);
socket.SetBlocking();
int curNumErr = 0;
while (!exitRequested)
{
b3Clock::usleep(0);
if ((pClient = socket.Accept()) != NULL)
{
b3AlignedObjectArray<char> bytesReceived;
int clientPort = socket.GetClientPort();
printf("connected from %s:%d\n", socket.GetClientAddr(),clientPort);
//----------------------------------------------------------------------
// Receive request from the client.
//----------------------------------------------------------------------
while (1)
{
//printf("try receive\n");
bool receivedData = false;
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))
{
curNumErr = 0;
char* msg2 = (char*) pClient->GetData();
int numBytesRec2 = pClient->GetBytesReceived();
int curSize = bytesReceived.size();
bytesReceived.resize(bytesReceived.size()+numBytesRec2);
for (int i=0;i<numBytesRec2;i++)
{
bytesReceived[curSize+i] = msg2[i];
}
if (bytesReceived.size() == 4 || bytesReceived.size()==sizeof(SharedMemoryCommand))
{
int numBytesRec = bytesReceived.size();
if (gVerboseNetworkMessagesServer)
{
printf("received message length [%d]\n",numBytesRec);
}
receivedData = true;
if (strncmp(&bytesReceived[0],"stop",4)==0)
{
printf("Stop request received\n");
exitRequested = true;
bytesReceived.clear();
break;
}
SharedMemoryCommand cmd;
SharedMemoryCommand* cmdPtr = 0;
//performance test
if (numBytesRec == sizeof(int))
{
cmdPtr = &cmd;
cmd.m_type = *(int*)&bytesReceived[0];
}
if (numBytesRec == sizeof(SharedMemoryCommand))
{
cmdPtr = (SharedMemoryCommand*)&bytesReceived[0];
}
if (cmdPtr)
{
SharedMemoryStatus serverStatus;
b3AlignedObjectArray<char> buffer;
buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
bool hasStatus = sm->processCommand(*cmdPtr,serverStatus, &buffer[0], buffer.size());
double startTimeSeconds = clock.getTimeInSeconds();
double curTimeSeconds = clock.getTimeInSeconds();
while ((!hasStatus) && ((curTimeSeconds - startTimeSeconds) <timeOutInSeconds))
{
hasStatus = sm->receiveStatus(serverStatus, &buffer[0], buffer.size());
curTimeSeconds = clock.getTimeInSeconds();
}
if (gVerboseNetworkMessagesServer)
{
printf("buffer.size = %d\n", buffer.size());
printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
}
if (hasStatus)
{
b3AlignedObjectArray<unsigned char> packetData;
unsigned char* statBytes = (unsigned char*)&serverStatus;
if (cmdPtr->m_type == CMD_STEP_FORWARD_SIMULATION)
{
packetData.resize(4 + sizeof(int));
int sz = packetData.size();
int curPos = 0;
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < sizeof(int); i++)
{
packetData[i + curPos] = statBytes[i];
}
curPos += sizeof(int);
pClient->Send( &packetData[0], packetData.size() );
}
else
{
//create packetData with [int packetSizeInBytes, status, streamBytes)
packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes);
int sz = packetData.size();
int curPos = 0;
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < sizeof(SharedMemoryStatus); i++)
{
packetData[i + curPos] = statBytes[i];
}
curPos += sizeof(SharedMemoryStatus);
for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++)
{
packetData[i + curPos] = buffer[i];
}
pClient->Send( &packetData[0], packetData.size() );
}
}
}
else
{
printf("received packet with unknown contents\n");
}
bytesReceived.clear();
}
}
if (!receivedData)
{
//printf("Didn't receive data.\n");
}
}
printf("Disconnecting client.\n");
pClient->Close();
delete pClient;
}
}
socket.Close();
socket.Shutdown(CSimpleSocket::Both);
}
delete sm;
return 0;
}

View File

@@ -0,0 +1,141 @@
project ("App_PhysicsServerSharedMemoryBridgeTCP")
language "C++"
kind "ConsoleApp"
includedirs {"../../ThirdPartyLibs/clsocket/src","../../../src",".."}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
links {
"clsocket",
"BulletFileLoader",
"Bullet3Common",
"LinearMath"
}
files {
"main.cpp",
"../PhysicsClient.cpp",
"../PhysicsClient.h",
"../PhysicsDirect.cpp",
"../PhysicsDirect.h",
"../PhysicsCommandProcessorInterface.h",
"../SharedMemoryCommandProcessor.cpp",
"../SharedMemoryCommandProcessor.h",
"../PhysicsClientC_API.cpp",
"../PhysicsClientC_API.h",
"../Win32SharedMemory.cpp",
"../Win32SharedMemory.h",
"../PosixSharedMemory.cpp",
"../PosixSharedMemory.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3ResourcePath.h",
"../../Utils/b3Clock.cpp",
"../../Utils/b3Clock.h",
}
project "App_PhysicsServerTCP"
if _OPTIONS["ios"] then
kind "WindowedApp"
else
kind "ConsoleApp"
end
defines { "NO_SHARED_MEMORY" }
includedirs {"..","../../../src", "../../ThirdPartyLibs","../../ThirdPartyLibs/clsocket/src"}
links {
"clsocket","Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
language "C++"
myfiles =
{
"../IKTrajectoryHelper.cpp",
"../IKTrajectoryHelper.h",
"../SharedMemoryCommands.h",
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../TinyRendererVisualShapeConverter.cpp",
"../TinyRendererVisualShapeConverter.h",
"../../TinyRenderer/geometry.cpp",
"../../TinyRenderer/model.cpp",
"../../TinyRenderer/tgaimage.cpp",
"../../TinyRenderer/our_gl.cpp",
"../../TinyRenderer/TinyRenderer.cpp",
"../../OpenGLWindow/SimpleCamera.cpp",
"../../OpenGLWindow/SimpleCamera.h",
"../../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
"../../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
"../../Importers/ImportMJCFDemo/BulletMJCFImporter.h",
"../../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../../Importers/ImportURDFDemo/UrdfParser.cpp",
"../../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../Importers/ImportURDFDemo/UrdfParser.cpp",
"../../Importers/ImportURDFDemo/UrdfParser.h",
"../../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../Importers/ImportURDFDemo/URDF2Bullet.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/RobotLoggingUtil.cpp",
"../../Utils/RobotLoggingUtil.h",
"../../../Extras/Serialize/BulletWorldImporter/*",
"../../../Extras/Serialize/BulletFileLoader/*",
"../../Importers/ImportURDFDemo/URDFImporterInterface.h",
"../../Importers/ImportURDFDemo/URDFJointTypes.h",
"../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
"../../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
"../../Importers/ImportSTLDemo/ImportSTLSetup.h",
"../../Importers/ImportSTLDemo/LoadMeshFromSTL.h",
"../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../ThirdPartyLibs/stb_image/stb_image.cpp",
}
files {
myfiles,
"main.cpp",
}

View File

@@ -133,9 +133,11 @@ int main(int argc, char *argv[])
{
if (gVerboseNetworkMessagesServer)
{
int dataLen = (int)event.packet->dataLength;
printf("A packet of length %u containing '%s' was "
"received from %s on channel %u.\n",
event.packet->dataLength,
dataLen,
event.packet->data,
event.peer->data,
event.channelID);