mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
34fc2fb589
add missing 1.sdf for Bullet/data/kitchens/1.sdf add support for getting keyboard events (pybullet.getKeyboardEvents and b3RobotSimulatorClientAPI::getKeyboardEvents)
630 lines
13 KiB
C++
630 lines
13 KiB
C++
#include "PhysicsClientUDP.h"
|
|
#include <enet/enet.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 "../MultiThreading/b3ThreadSupportInterface.h"
|
|
void UDPThreadFunc(void* userPtr, void* lsMemory);
|
|
void* UDPlsMemoryFunc();
|
|
bool gVerboseNetworkMessagesClient = false;
|
|
|
|
#ifndef _WIN32
|
|
#include "../MultiThreading/b3PosixThreadSupport.h"
|
|
|
|
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
|
|
{
|
|
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("UDPThread",
|
|
UDPThreadFunc,
|
|
UDPlsMemoryFunc,
|
|
numThreads);
|
|
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
|
|
|
|
return threadSupport;
|
|
|
|
}
|
|
|
|
|
|
#elif defined( _WIN32)
|
|
#include "../MultiThreading/b3Win32ThreadSupport.h"
|
|
|
|
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
|
|
{
|
|
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc, numThreads);
|
|
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
|
|
return threadSupport;
|
|
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
struct UDPThreadLocalStorage
|
|
{
|
|
int threadId;
|
|
};
|
|
|
|
|
|
|
|
unsigned int b3DeserializeInt(const unsigned char* input)
|
|
{
|
|
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
|
|
return tmp;
|
|
}
|
|
|
|
struct UdpNetworkedInternalData
|
|
{
|
|
ENetHost* m_client;
|
|
ENetAddress m_address;
|
|
ENetPeer* m_peer;
|
|
ENetEvent m_event;
|
|
bool m_isConnected;
|
|
|
|
b3ThreadSupportInterface* m_threadSupport;
|
|
|
|
b3CriticalSection* m_cs;
|
|
|
|
UdpNetworkedInternalData* m_udpInternalData;
|
|
|
|
|
|
SharedMemoryCommand m_clientCmd;
|
|
bool m_hasCommand;
|
|
|
|
bool m_hasStatus;
|
|
SharedMemoryStatus m_lastStatus;
|
|
b3AlignedObjectArray<char> m_stream;
|
|
|
|
std::string m_hostName;
|
|
int m_port;
|
|
double m_timeOutInSeconds;
|
|
|
|
UdpNetworkedInternalData()
|
|
:m_client(0),
|
|
m_peer(0),
|
|
m_isConnected(false),
|
|
m_threadSupport(0),
|
|
m_hasCommand(false),
|
|
m_hasStatus(false),
|
|
m_timeOutInSeconds(60)
|
|
{
|
|
|
|
}
|
|
|
|
bool connectUDP()
|
|
{
|
|
if (m_isConnected)
|
|
return true;
|
|
|
|
|
|
|
|
if (enet_initialize() != 0)
|
|
{
|
|
fprintf(stderr, "Error initialising enet");
|
|
|
|
exit(EXIT_FAILURE);
|
|
|
|
}
|
|
|
|
m_client = enet_host_create(NULL, /* create a client host */
|
|
1, /* number of clients */
|
|
2, /* number of channels */
|
|
57600 / 8, /* incoming bandwith */
|
|
14400 / 8); /* outgoing bandwith */
|
|
|
|
if (m_client == NULL) {
|
|
fprintf(stderr, "Could not create client host");
|
|
return false;
|
|
}
|
|
|
|
enet_address_set_host(&m_address, m_hostName.c_str());
|
|
m_address.port = m_port;
|
|
|
|
m_peer = enet_host_connect(m_client,
|
|
&m_address, /* address to connect to */
|
|
2, /* number of channels */
|
|
0); /* user data supplied to
|
|
the receiving host */
|
|
|
|
if (m_peer == NULL) {
|
|
fprintf(stderr, "No available peers for initiating an ENet "
|
|
"connection.\n");
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
/* Try to connect to server within 5 seconds */
|
|
if (enet_host_service(m_client, &m_event, 5000) > 0 &&
|
|
m_event.type == ENET_EVENT_TYPE_CONNECT)
|
|
{
|
|
puts("Connection to server succeeded.");
|
|
}
|
|
else
|
|
{
|
|
/* Either the 5 seconds are up or a disconnect event was */
|
|
/* received. Reset the peer in the event the 5 seconds */
|
|
/* had run out without any significant event. */
|
|
enet_peer_reset(m_peer);
|
|
|
|
fprintf(stderr, "Connection to server failed.");
|
|
return false;
|
|
}
|
|
|
|
|
|
int serviceResult = enet_host_service(m_client, &m_event, 0);
|
|
|
|
if (serviceResult > 0)
|
|
{
|
|
switch (m_event.type)
|
|
{
|
|
case ENET_EVENT_TYPE_CONNECT:
|
|
printf("A new client connected from %x:%u.\n",
|
|
m_event.peer->address.host,
|
|
m_event.peer->address.port);
|
|
m_event.peer->data = (void*)"New User";
|
|
break;
|
|
|
|
case ENET_EVENT_TYPE_RECEIVE:
|
|
|
|
if (gVerboseNetworkMessagesClient)
|
|
{
|
|
printf("A packet of length %lu containing '%s' was "
|
|
"received from %s on channel %u.\n",
|
|
m_event.packet->dataLength,
|
|
(char*)m_event.packet->data,
|
|
(char*)m_event.peer->data,
|
|
m_event.channelID);
|
|
}
|
|
/* Clean up the packet now that we're done using it.
|
|
> */
|
|
enet_packet_destroy(m_event.packet);
|
|
|
|
break;
|
|
|
|
case ENET_EVENT_TYPE_DISCONNECT:
|
|
printf("%s disconnected.\n", (char*)m_event.peer->data);
|
|
|
|
break;
|
|
default:
|
|
{
|
|
printf("unknown event type: %d.\n", m_event.type);
|
|
}
|
|
}
|
|
}
|
|
else if (serviceResult > 0)
|
|
{
|
|
puts("Error with servicing the client");
|
|
return false;
|
|
}
|
|
|
|
m_isConnected = true;
|
|
return m_isConnected;
|
|
}
|
|
|
|
bool checkData()
|
|
{
|
|
bool hasStatus = false;
|
|
|
|
int serviceResult = enet_host_service(m_client, &m_event, 0);
|
|
|
|
if (serviceResult > 0)
|
|
{
|
|
switch (m_event.type)
|
|
{
|
|
case ENET_EVENT_TYPE_CONNECT:
|
|
printf("A new client connected from %x:%u.\n",
|
|
m_event.peer->address.host,
|
|
m_event.peer->address.port);
|
|
|
|
m_event.peer->data = (void*)"New User";
|
|
break;
|
|
|
|
case ENET_EVENT_TYPE_RECEIVE:
|
|
{
|
|
if (gVerboseNetworkMessagesClient)
|
|
{
|
|
printf("A packet of length %lu containing '%s' was "
|
|
"received from %s on channel %u.\n",
|
|
m_event.packet->dataLength,
|
|
(char*)m_event.packet->data,
|
|
(char*)m_event.peer->data,
|
|
m_event.channelID);
|
|
}
|
|
|
|
int packetSizeInBytes = b3DeserializeInt(m_event.packet->data);
|
|
|
|
if (packetSizeInBytes == m_event.packet->dataLength)
|
|
{
|
|
|
|
SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&m_event.packet->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] = m_event.packet->data[i + streamOffsetInBytes];
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
printf("unknown status message received\n");
|
|
}
|
|
enet_packet_destroy(m_event.packet);
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
case ENET_EVENT_TYPE_DISCONNECT:
|
|
{
|
|
printf("%s disconnected.\n", (char*)m_event.peer->data);
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
printf("unknown event type: %d.\n", m_event.type);
|
|
}
|
|
}
|
|
}
|
|
else if (serviceResult > 0)
|
|
{
|
|
puts("Error with servicing the client");
|
|
}
|
|
|
|
return hasStatus;
|
|
}
|
|
|
|
};
|
|
|
|
enum UDPThreadEnums
|
|
{
|
|
eUDPRequestTerminate = 13,
|
|
eUDPIsUnInitialized,
|
|
eUDPIsInitialized,
|
|
eUDPInitializationFailed,
|
|
eUDPHasTerminated
|
|
};
|
|
|
|
|
|
|
|
enum UDPCommandEnums
|
|
{
|
|
eUDPIdle = 13,
|
|
eUDP_ConnectRequest,
|
|
eUDP_Connected,
|
|
eUDP_ConnectionFailed,
|
|
eUDP_DisconnectRequest,
|
|
eUDP_Disconnected,
|
|
|
|
};
|
|
|
|
void UDPThreadFunc(void* userPtr, void* lsMemory)
|
|
{
|
|
printf("UDPThreadFunc thread started\n");
|
|
// UDPThreadLocalStorage* localStorage = (UDPThreadLocalStorage*)lsMemory;
|
|
|
|
UdpNetworkedInternalData* args = (UdpNetworkedInternalData*)userPtr;
|
|
// int workLeft = true;
|
|
b3Clock clock;
|
|
clock.reset();
|
|
bool init = true;
|
|
if (init)
|
|
{
|
|
|
|
args->m_cs->lock();
|
|
args->m_cs->setSharedParam(0, eUDPIsInitialized);
|
|
args->m_cs->unlock();
|
|
|
|
|
|
double deltaTimeInSeconds = 0;
|
|
|
|
do
|
|
{
|
|
b3Clock::usleep(0);
|
|
|
|
deltaTimeInSeconds += double(clock.getTimeMicroseconds()) / 1000000.;
|
|
|
|
{
|
|
|
|
clock.reset();
|
|
deltaTimeInSeconds = 0.f;
|
|
switch (args->m_cs->getSharedParam(1))
|
|
{
|
|
case eUDP_ConnectRequest:
|
|
{
|
|
bool connected = args->connectUDP();
|
|
if (connected)
|
|
{
|
|
args->m_cs->setSharedParam(1, eUDP_Connected);
|
|
}
|
|
else
|
|
{
|
|
args->m_cs->setSharedParam(1, eUDP_ConnectionFailed);
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
};
|
|
|
|
if (args->m_isConnected)
|
|
{
|
|
|
|
args->m_cs->lock();
|
|
bool hasCommand = args->m_hasCommand;
|
|
args->m_cs->unlock();
|
|
|
|
|
|
if (hasCommand)
|
|
{
|
|
int sz = 0;
|
|
ENetPacket *packet = 0;
|
|
|
|
if (args->m_clientCmd.m_type == CMD_STEP_FORWARD_SIMULATION)
|
|
{
|
|
sz = sizeof(int);
|
|
packet = enet_packet_create(&args->m_clientCmd.m_type, sz, ENET_PACKET_FLAG_RELIABLE);
|
|
}
|
|
else
|
|
{
|
|
sz = sizeof(SharedMemoryCommand);
|
|
packet = enet_packet_create(&args->m_clientCmd, sz, ENET_PACKET_FLAG_RELIABLE);
|
|
}
|
|
int res;
|
|
res = enet_peer_send(args->m_peer, 0, packet);
|
|
args->m_cs->lock();
|
|
args->m_hasCommand = false;
|
|
args->m_cs->unlock();
|
|
}
|
|
|
|
|
|
bool hasNewStatus = args->checkData();
|
|
if (hasNewStatus)
|
|
{
|
|
if (args->m_hasStatus)
|
|
{
|
|
//overflow: last status hasn't been processed yet
|
|
b3Assert(0);
|
|
printf("Error: received new status but previous status not processed yet");
|
|
}
|
|
else
|
|
{
|
|
args->m_cs->lock();
|
|
args->m_hasStatus = hasNewStatus;
|
|
args->m_cs->unlock();
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
} while (args->m_cs->getSharedParam(0) != eUDPRequestTerminate);
|
|
}
|
|
else
|
|
{
|
|
args->m_cs->lock();
|
|
args->m_cs->setSharedParam(0, eUDPInitializationFailed);
|
|
args->m_cs->unlock();
|
|
}
|
|
|
|
|
|
printf("finished\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
void* UDPlsMemoryFunc()
|
|
{
|
|
//don't create local store memory, just return 0
|
|
return new UDPThreadLocalStorage;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
UdpNetworkedPhysicsProcessor::UdpNetworkedPhysicsProcessor(const char* hostName, int port)
|
|
{
|
|
m_data = new UdpNetworkedInternalData;
|
|
if (hostName)
|
|
{
|
|
m_data->m_hostName = hostName;
|
|
}
|
|
m_data->m_port = port;
|
|
|
|
}
|
|
|
|
UdpNetworkedPhysicsProcessor::~UdpNetworkedPhysicsProcessor()
|
|
{
|
|
disconnect();
|
|
delete m_data;
|
|
}
|
|
|
|
bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
|
{
|
|
if(gVerboseNetworkMessagesClient)
|
|
{
|
|
printf("PhysicsClientUDP::processCommand\n");
|
|
}
|
|
// int sz = sizeof(SharedMemoryCommand);
|
|
|
|
b3Clock clock;
|
|
double startTime = clock.getTimeInSeconds();
|
|
double timeOutInSeconds = m_data->m_timeOutInSeconds;
|
|
|
|
m_data->m_cs->lock();
|
|
m_data->m_clientCmd = clientCmd;
|
|
m_data->m_hasCommand = true;
|
|
m_data->m_cs->unlock();
|
|
|
|
while ((m_data->m_hasCommand) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
|
{
|
|
b3Clock::usleep(0);
|
|
}
|
|
|
|
#if 0
|
|
|
|
|
|
bool hasStatus = false;
|
|
|
|
b3Clock clock;
|
|
double startTime = clock.getTimeInSeconds();
|
|
double timeOutInSeconds = m_data->m_timeOutInSeconds;
|
|
|
|
const SharedMemoryStatus* stat = 0;
|
|
while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
|
|
{
|
|
hasStatus = receiveStatus(serverStatusOut, bufferServerToClient, bufferSizeInBytes);
|
|
b3Clock::usleep(100);
|
|
}
|
|
return hasStatus;
|
|
|
|
#endif
|
|
|
|
return false;
|
|
}
|
|
|
|
bool UdpNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
|
{
|
|
bool hasStatus = false;
|
|
if (m_data->m_hasStatus)
|
|
{
|
|
if (gVerboseNetworkMessagesClient)
|
|
{
|
|
printf("UdpNetworkedPhysicsProcessor::receiveStatus\n");
|
|
}
|
|
|
|
hasStatus = true;
|
|
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");
|
|
}
|
|
|
|
m_data->m_cs->lock();
|
|
m_data->m_hasStatus = false;
|
|
m_data->m_cs->unlock();
|
|
}
|
|
|
|
|
|
return hasStatus;
|
|
|
|
}
|
|
|
|
|
|
void UdpNetworkedPhysicsProcessor::renderScene()
|
|
{
|
|
}
|
|
|
|
void UdpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
|
|
{
|
|
}
|
|
|
|
void UdpNetworkedPhysicsProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
|
|
{
|
|
}
|
|
|
|
bool UdpNetworkedPhysicsProcessor::isConnected() const
|
|
{
|
|
return m_data->m_isConnected;
|
|
}
|
|
|
|
|
|
bool UdpNetworkedPhysicsProcessor::connect()
|
|
{
|
|
if (m_data->m_threadSupport==0)
|
|
{
|
|
m_data->m_threadSupport = createUDPThreadSupport(1);
|
|
|
|
m_data->m_cs = m_data->m_threadSupport->createCriticalSection();
|
|
m_data->m_cs->setSharedParam(0, eUDPIsUnInitialized);
|
|
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) m_data, 0);
|
|
|
|
while (m_data->m_cs->getSharedParam(0) == eUDPIsUnInitialized)
|
|
{
|
|
b3Clock::usleep(1000);
|
|
}
|
|
|
|
m_data->m_cs->lock();
|
|
m_data->m_cs->setSharedParam(1, eUDP_ConnectRequest);
|
|
m_data->m_cs->unlock();
|
|
|
|
while (m_data->m_cs->getSharedParam(1) == eUDP_ConnectRequest)
|
|
{
|
|
b3Clock::usleep(1000);
|
|
}
|
|
|
|
}
|
|
unsigned int sharedParam = m_data->m_cs->getSharedParam(1);
|
|
bool isConnected = (sharedParam == eUDP_Connected);
|
|
return isConnected;
|
|
}
|
|
|
|
void UdpNetworkedPhysicsProcessor::disconnect()
|
|
{
|
|
if (m_data->m_threadSupport)
|
|
{
|
|
m_data->m_cs->lock();
|
|
m_data->m_cs->setSharedParam(0, eUDPRequestTerminate);
|
|
m_data->m_cs->unlock();
|
|
|
|
int numActiveThreads = 1;
|
|
|
|
while (numActiveThreads)
|
|
{
|
|
int arg0, arg1;
|
|
if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
|
|
{
|
|
numActiveThreads--;
|
|
printf("numActiveThreads = %d\n", numActiveThreads);
|
|
}
|
|
else
|
|
{
|
|
b3Clock::usleep(1000);
|
|
}
|
|
};
|
|
|
|
printf("stopping threads\n");
|
|
|
|
delete m_data->m_threadSupport;
|
|
m_data->m_threadSupport = 0;
|
|
m_data->m_isConnected = false;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
void UdpNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds)
|
|
{
|
|
m_data->m_timeOutInSeconds = timeOutInSeconds;
|
|
}
|