From 49b0ec08e1e7cc7e738443a1cd22764e9a3f0eeb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 22 Jun 2018 16:47:20 -0700 Subject: [PATCH] PyBullet: allow to replace existing debug lines in addUserDebugLine through the replaceItemUniqueId argument See batchRayCast.py for example usage --- .../CommonGUIHelperInterface.h | 2 +- examples/ExampleBrowser/OpenGLGuiHelper.h | 3 +- examples/SharedMemory/PhysicsClientC_API.cpp | 1 - .../PhysicsServerCommandProcessor.cpp | 10 ++++++- .../SharedMemory/PhysicsServerExample.cpp | 28 +++++++++++++---- examples/pybullet/examples/batchRayCast.py | 30 ++++++++++++++----- examples/pybullet/pybullet.c | 11 +++++-- 7 files changed, 66 insertions(+), 19 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index a9e273f02..de3172d18 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -103,7 +103,7 @@ struct GUIHelperInterface virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag){} virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags, int replaceItemUid){return -1;} - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex){return -1;}; + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid){return -1;}; virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue){return -1;}; virtual int readUserDebugParameter(int itemUniqueId, double* value) { return 0;} diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 3a43a0221..cf5a4c4f4 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -75,7 +75,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface { return -1; } - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid ) { return -1; } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 35f487d7b..e5abaa26a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3063,7 +3063,6 @@ B3_SHARED_API void b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHa struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); - b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT); command->m_userDebugDrawArgs.m_replaceItemUniqueId = replaceItemUniqueId; command->m_updateFlags |= USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ee134e9cd..f89135df2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4582,13 +4582,21 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) { + int replaceItemUid = -1; + if (clientCmd.m_updateFlags&USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID) + { + replaceItemUid = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; + } + int uid = m_data->m_guiHelper->addUserDebugLine( clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, clientCmd.m_userDebugDrawArgs.m_lineWidth, clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex); + trackingVisualShapeIndex, + replaceItemUid + ); if (uid>=0) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index fd4b3bd10..5260d6356 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -515,6 +515,7 @@ struct UserDebugDrawLine double m_lifeTime; int m_itemUniqueId; int m_trackingVisualShapeIndex; + int m_replaceItemUid; }; struct UserDebugParameter @@ -1275,11 +1276,14 @@ public: btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; int m_resultDebugLineUid; - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) + + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid) { m_tmpLine.m_lifeTime = lifeTime; m_tmpLine.m_lineWidth = lineWidth; - m_tmpLine.m_itemUniqueId = m_uidGenerator++; + + m_tmpLine.m_itemUniqueId = replaceItemUid<0? m_uidGenerator++ : replaceItemUid; m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; @@ -1292,6 +1296,7 @@ public: m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex; + m_tmpLine.m_replaceItemUid = replaceItemUid; m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); m_resultDebugLineUid=-1; @@ -2365,10 +2370,23 @@ void PhysicsServerExample::updateGraphics() { B3_PROFILE("eGUIUserDebugAddLine"); - m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); - m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; + if (m_multiThreadedHelper->m_tmpLine.m_replaceItemUid>=0) + { + for (int i=0;im_userDebugLines.size();i++) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpLine.m_replaceItemUid) + { + m_multiThreadedHelper->m_userDebugLines[i] = m_multiThreadedHelper->m_tmpLine; + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_tmpLine.m_replaceItemUid; + } + } + } else + { + m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; + } m_multiThreadedHelper->mainThreadRelease(); - break; + break; } case eGUIUserDebugRemoveItem: { diff --git a/examples/pybullet/examples/batchRayCast.py b/examples/pybullet/examples/batchRayCast.py index f25cf4cff..44b262dba 100644 --- a/examples/pybullet/examples/batchRayCast.py +++ b/examples/pybullet/examples/batchRayCast.py @@ -9,22 +9,34 @@ if (useGui): else: p.connect(p.DIRECT) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) -p.loadURDF("samurai.urdf") +#p.loadURDF("samurai.urdf") p.loadURDF("r2d2.urdf",[3,3,1]) rayFrom=[] rayTo=[] +rayIds=[] numRays = 1024 + rayLen = 13 +rayHitColor = [1,0,0] +rayMissColor = [0,1,0] + +replaceLines = True + for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) + if (replaceLines): + rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) + else: + rayIds.append(-1) if (not useGui): timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") @@ -38,24 +50,26 @@ for i in range (numSteps): for j in range (8): results = p.rayTestBatch(rayFrom,rayTo,j+1) - for i in range (10): - p.removeAllUserDebugItems() + #for i in range (10): + # p.removeAllUserDebugItems() - rayHitColor = [1,0,0] - rayMissColor = [0,1,0] + if (useGui): - p.removeAllUserDebugItems() + if (not replaceLines): + p.removeAllUserDebugItems() + for i in range (numRays): hitObjectUid=results[i][0] if (hitObjectUid<0): - p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor) + hitPosition =[0,0,0] + p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i]) else: hitPosition = results[i][3] - p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor) + p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor,replaceItemUniqueId=rayIds[i]) #time.sleep(1./240.) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fb6078865..b7c704305 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4261,10 +4261,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double lifeTime = 0.f; int physicsClientId = 0; int debugItemUniqueId = -1; + int replaceItemUniqueId = -1; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "replaceItemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &replaceItemUniqueId, &physicsClientId)) { return NULL; } @@ -4293,6 +4294,7 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj res = pybullet_internalSetVectord(lineColorRGBObj, colorRGB); } + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); if (parentObjectUniqueId>=0) @@ -4300,6 +4302,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); } + if (replaceItemUniqueId>=0) + { + b3UserDebugItemSetReplaceItemUniqueId(commandHandle,replaceItemUniqueId); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)