From 815a56c9bc73ec5a3f9d0644f3d82a7c2fe6a168 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 23 Sep 2017 09:25:00 -0700 Subject: [PATCH] Allow to load a urdf file in the testplugin.cpp, as first quick test, example pybullet script: import pybullet as p p.connect(p.GUI) pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll") commandUid = 0 argument = "plane.urdf" p.executePluginCommand(pluginUid,commandUid,argument) p.unloadPlugin(pluginUid) --- .../PhysicsClientSharedMemory.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 7 +++--- examples/SharedMemory/b3PluginManager.cpp | 13 +++++++++-- examples/SharedMemory/b3PluginManager.h | 2 +- examples/SharedMemory/plugins/b3PluginAPI.h | 2 +- .../SharedMemory/plugins/b3PluginContext.h | 13 +++++++++++ .../plugins/testPlugin/testplugin.cpp | 22 +++++++++++++++---- .../plugins/testPlugin/testplugin.h | 2 +- 8 files changed, 50 insertions(+), 13 deletions(-) create mode 100644 examples/SharedMemory/plugins/b3PluginContext.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 13b75eb8b..1ea576f65 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData { m_hasLastServerStatus(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false), - m_timeOutInSeconds(5) + m_timeOutInSeconds(30) {} void processServerStatus(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 97be495d9..4d95946ad 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1516,8 +1516,8 @@ struct PhysicsServerCommandProcessorInternalData b3HashMap m_profileEvents; - PhysicsServerCommandProcessorInternalData() - : + PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) + :m_pluginManager(proc), m_allowRealTimeSimulation(false), m_commandLogger(0), m_logPlayback(0), @@ -1647,8 +1647,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() + :m_data(0) { - m_data = new PhysicsServerCommandProcessorInternalData(); + m_data = new PhysicsServerCommandProcessorInternalData(this); createEmptyDynamicsWorld(); diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 2590a6a96..7a665b1e4 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -4,6 +4,8 @@ #include "Bullet3Common/b3ResizablePool.h" #include "plugins/b3PluginAPI.h" #include "SharedMemoryPublic.h" +#include "PhysicsDirect.h" +#include "plugins/b3PluginContext.h" #ifdef _WIN32 #define WIN32_LEAN_AND_MEAN @@ -50,15 +52,19 @@ struct b3PluginManagerInternalData { b3ResizablePool m_plugins; b3HashMap m_pluginMap; + PhysicsDirect* m_physicsDirect; }; -b3PluginManager::b3PluginManager() +b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk) { m_data = new b3PluginManagerInternalData; + m_data->m_physicsDirect = new PhysicsDirect(physSdk,false); + } b3PluginManager::~b3PluginManager() { + delete m_data->m_physicsDirect; m_data->m_pluginMap.clear(); m_data->m_plugins.exitHandles(); delete m_data; @@ -145,7 +151,10 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const char* argume b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); if (plugin) { - result = plugin->m_executeCommandFunc(arguments); + b3PluginContext context; + context.m_arguments = arguments; + context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + result = plugin->m_executeCommandFunc(&context); } return result; } diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h index be8979c95..9a71b352a 100644 --- a/examples/SharedMemory/b3PluginManager.h +++ b/examples/SharedMemory/b3PluginManager.h @@ -7,7 +7,7 @@ class b3PluginManager public: - b3PluginManager(); + b3PluginManager(class PhysicsCommandProcessorInterface* physSdk); virtual ~b3PluginManager(); int loadPlugin(const char* pluginPath); diff --git a/examples/SharedMemory/plugins/b3PluginAPI.h b/examples/SharedMemory/plugins/b3PluginAPI.h index 408517ddb..de5da762d 100644 --- a/examples/SharedMemory/plugins/b3PluginAPI.h +++ b/examples/SharedMemory/plugins/b3PluginAPI.h @@ -28,7 +28,7 @@ extern "C" { /* Plugin API */ typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(); typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(); - typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(const char* arguments); + typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context); #ifdef __cplusplus } diff --git a/examples/SharedMemory/plugins/b3PluginContext.h b/examples/SharedMemory/plugins/b3PluginContext.h new file mode 100644 index 000000000..873269f39 --- /dev/null +++ b/examples/SharedMemory/plugins/b3PluginContext.h @@ -0,0 +1,13 @@ +#ifndef B3_PLUGIN_CONTEXT_H +#define B3_PLUGIN_CONTEXT_H + +#include "../PhysicsClientC_API.h" + +struct b3PluginContext +{ + const char* m_arguments; + + b3PhysicsClientHandle m_physClient; +}; + +#endif //B3_PLUGIN_CONTEXT_H \ No newline at end of file diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.cpp b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp index 41482212a..1b723d380 100644 --- a/examples/SharedMemory/plugins/testPlugin/testplugin.cpp +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp @@ -1,7 +1,7 @@ #include "testplugin.h" #include "../../SharedMemoryPublic.h" - +#include "../b3PluginContext.h" #include B3_SHARED_API int initPlugin() @@ -10,10 +10,24 @@ B3_SHARED_API int initPlugin() return SHARED_MEMORY_MAGIC_NUMBER; } -B3_SHARED_API int executePluginCommand(const char* arguments) +B3_SHARED_API int executePluginCommand(struct b3PluginContext* context) { - printf("arguments:%s\n",arguments); - return 42; + printf("arguments:%s\n",context->m_arguments); + + b3SharedMemoryStatusHandle statusHandle; + int statusType = -1; + int bodyUniqueId = -1; + + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(context->m_physClient, context->m_arguments); + + statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + bodyUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return bodyUniqueId; } diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.h b/examples/SharedMemory/plugins/testPlugin/testplugin.h index 2b4809617..53718693f 100644 --- a/examples/SharedMemory/plugins/testPlugin/testplugin.h +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.h @@ -10,7 +10,7 @@ extern "C" B3_SHARED_API int initPlugin(); B3_SHARED_API void exitPlugin(); -B3_SHARED_API int executePluginCommand(const char* arguments); +B3_SHARED_API int executePluginCommand(struct b3PluginContext* context); #ifdef __cplusplus };