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

preparation to receive camera image data from physics server

Browse Source 
increase shadowmap world size default to 50 units (meter), 10 units (meter) was too small for most examples.
This commit is contained in:
erwin coumans
2016-05-31 10:23:04 -07:00
parent de9bd65c19
commit 14aa666c6f
21 changed files with 301 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/SharedMemory/PhysicsClient.h
View File

@@ -37,6 +37,9 @@ public:
virtual const float* getDebugLinesFrom() const = 0;
virtual const float* getDebugLinesTo() const = 0;
virtual const float* getDebugLinesColor() const = 0;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
};
#endif // BT_PHYSICS_CLIENT_API_H

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -686,13 +686,10 @@ b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physC
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_CAMERA_IMAGE_DATA;
command->m_requestPixelDataArguments.m_startPixelIndex = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3RequestCameraImageSetResolution(b3SharedMemoryCommandHandle command, int pixelWidth, int pixelHeight)
{
}
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16])
{
@@ -704,6 +701,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedCameraImage(imageData);
}
}

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -67,7 +67,6 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetResolution(b3SharedMemoryCommandHandle command, int pixelWidth, int pixelHeight);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);

69
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -2,7 +2,7 @@
#include "PhysicsClientExample.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClientC_API.h"
@@ -21,6 +21,9 @@ struct MyMotorInfo2
int m_qIndex;
};
int camVisualizerWidth = 1024/3;
int camVisualizerHeight = 768/3;
#define MAX_NUM_MOTORS 128
@@ -37,6 +40,9 @@ protected:
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
struct Common2dCanvasInterface* m_canvas;
int m_canvasIndex;
void createButton(const char* name, int id, bool isTrigger );
void createButtons();
@@ -401,7 +407,8 @@ m_selectedBody(-1),
m_prevSelectedBody(-1),
m_numMotors(0),
m_options(options),
m_isOptionalServerConnected(false)
m_isOptionalServerConnected(false),
m_canvas(0)
{
b3Printf("Started PhysicsClientExample\n");
}
@@ -518,6 +525,34 @@ void PhysicsClientExample::initPhysics()
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
{
m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight);
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<camVisualizerHeight;j++)
{
unsigned char red=255;
unsigned char green=255;
unsigned char blue=255;
unsigned char alpha=255;
if (i==j)
{
red = 0;
green=0;
blue=0;
}
m_canvas->setPixel(m_canvasIndex,i,j,red,green,blue,alpha);
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
@@ -564,7 +599,35 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
b3Printf("Camera image OK\n");
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
imageData.m_rgbColorData[pixelIndex+3]);
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{

68
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -32,6 +32,11 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
SharedMemoryStatus m_lastServerStatus;
int m_counter;
@@ -46,8 +51,10 @@ struct PhysicsClientSharedMemoryInternalData {
: m_sharedMemory(0),
m_ownsSharedMemory(false),
m_testBlock1(0),
m_counter(0),
m_serverLoadUrdfOK(false),
m_counter(0),
m_cachedCameraPixelsWidth(0),
m_cachedCameraPixelsHeight(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -419,7 +426,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_COMPLETED:
{
b3Printf("Camera image OK\n");
if (m_data->m_verboseOutput)
{
b3Printf("Camera image OK\n");
}
int numBytesPerPixel = 4;//RGBA
int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
m_data->m_cachedCameraPixelsWidth = 0;
m_data->m_cachedCameraPixelsHeight = 0;
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
= rgbaPixelsReceived[i];
}
break;
}
@@ -445,6 +475,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = false;
} else {
m_data->m_waitingForServer = true;
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
{
// continue requesting remaining pixels
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
command.m_requestPixelDataArguments.m_startPixelIndex =
serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
submitClientCommand(command);
return 0;
} else
{
m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
}
}
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
@@ -508,6 +562,14 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
}
}
void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;

1
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -45,6 +45,7 @@ public:
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
};
#endif // BT_PHYSICS_CLIENT_API_H

11
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -328,3 +328,14 @@ const float* PhysicsDirect::getDebugLinesColor() const
}
return 0;
}
void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
if (cameraData)
{
cameraData->m_pixelHeight = 0;
cameraData->m_pixelWidth = 0;
cameraData->m_depthValues = 0;
cameraData->m_rgbColorData = 0;
}
}

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -57,6 +57,8 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
};
#endif //PHYSICS_DIRECT_H

5
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -113,3 +113,8 @@ const float* PhysicsLoopBack::getDebugLinesColor() const
{
return m_data->m_physicsClient->getDebugLinesColor();
}
void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
return m_data->m_physicsClient->getCachedCameraImage(cameraData);
}

1
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -52,6 +52,7 @@ public:
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
};

38
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -967,8 +967,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
int width, height;
int numPixelsCopied = 0;
m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0);
int numTotalPixels = width*height;
int numRemainingPixels = numTotalPixels - startPixelIndex;
if (numRemainingPixels>0)
{
int maxNumPixels = bufferSizeInBytes/8-1;
unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels);
float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
m_data->m_guiHelper->copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied);
//each pixel takes 4 RGBA values and 1 float = 8 bytes
} else
{
}
serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height;
hasStatus = true;
break;
}

17
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -113,6 +113,11 @@ struct RequestDebugLinesArgs
int m_startingLineIndex;
};
struct RequestPixelDataArgs
{
int m_startPixelIndex;
};
struct SendDebugLinesArgs
{
int m_startingLineIndex;
@@ -120,6 +125,16 @@ struct SendDebugLinesArgs
int m_numRemainingDebugLines;
};
struct SendPixelDataArgs
{
int m_imageWidth;
int m_imageHeight;
int m_startingPixelIndex;
int m_numPixelsCopied;
int m_numRemainingPixels;
};
struct PickBodyArgs
{
double m_rayFromWorld[3];
@@ -270,6 +285,7 @@ struct SharedMemoryCommand
struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments;
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
};
};
@@ -291,6 +307,7 @@ struct SharedMemoryStatus
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
};
};