From 1b03871b4d26d6aba2177ce928ecad5288aaee6d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 3 Oct 2017 18:01:53 -0700 Subject: [PATCH] use explicit size for name, to avoid issue converting/marshalling data to C# example to call b3VisualShapeInformation from C# and marshall b3VisualShapeData (after loadURDF) See examples\pybullet\unity3d\examples\NewBehaviourScript.cs --- examples/SharedMemory/BodyJointInfoUtility.h | 10 ++- examples/SharedMemory/PhysicsClientC_API.cpp | 5 +- .../PhysicsClientSharedMemory.cpp | 21 ++--- examples/SharedMemory/PhysicsDirect.cpp | 15 +--- examples/SharedMemory/SharedMemoryPublic.h | 8 +- .../pybullet/unity3d/autogen/NativeMethods.cs | 29 ++++--- .../unity3d/examples/NewBehaviourScript.cs | 80 ++++++++++--------- 7 files changed, 79 insertions(+), 89 deletions(-) diff --git a/examples/SharedMemory/BodyJointInfoUtility.h b/examples/SharedMemory/BodyJointInfoUtility.h index decced5ba..6bf018808 100644 --- a/examples/SharedMemory/BodyJointInfoUtility.h +++ b/examples/SharedMemory/BodyJointInfoUtility.h @@ -34,8 +34,8 @@ template void addJointInfoFromMultiBodyData(const T* mb { { b3JointInfo info; - info.m_jointName = 0; - info.m_linkName = 0; + info.m_jointName[0] = 0; + info.m_linkName[0] = 0; info.m_flags = 0; info.m_jointIndex = link; info.m_qIndex = @@ -48,14 +48,16 @@ template void addJointInfoFromMultiBodyData(const T* mb b3Printf("mb->m_links[%d].m_linkName = %s\n", link, mb->m_links[link].m_linkName); } - info.m_linkName = strDup(mb->m_links[link].m_linkName); + strcpy(info.m_linkName,mb->m_links[link].m_linkName); + } if (mb->m_links[link].m_jointName) { if (verboseOutput) { b3Printf("mb->m_links[%d].m_jointName = %s\n", link, mb->m_links[link].m_jointName); } - info.m_jointName = strDup(mb->m_links[link].m_jointName); + strcpy(info.m_jointName,mb->m_links[link].m_jointName); + //info.m_jointName = strDup(mb->m_links[link].m_jointName); } info.m_jointType = mb->m_links[link].m_jointType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a32e3dedc..ce39a2dd2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1438,7 +1438,10 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; - cl->disconnectSharedMemory(); + if (cl) + { + cl->disconnectSharedMemory(); + } delete cl; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 94c73f5f2..e3f133acf 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -108,8 +108,8 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; - info.m_baseName = bodyJoints->m_baseName.c_str(); - info.m_bodyName = bodyJoints->m_bodyName.c_str(); + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName,bodyJoints->m_bodyName.c_str()); return true; } @@ -234,16 +234,6 @@ void PhysicsClientSharedMemory::resetData() if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; - for (int j=0;jm_jointInfo.size();j++) { - if (bodyJoints->m_jointInfo[j].m_jointName) - { - free(bodyJoints->m_jointInfo[j].m_jointName); - } - if (bodyJoints->m_jointInfo[j].m_linkName) - { - free(bodyJoints->m_jointInfo[j].m_linkName); - } - } delete (*bodyJointsPtr); } } @@ -392,8 +382,8 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha template void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput) { b3JointInfo info; - info.m_jointName = 0; - info.m_linkName = 0; + info.m_jointName[0] = 0; + info.m_linkName[0] = 0; info.m_flags = 0; info.m_jointIndex = linkIndex; info.m_qIndex = linkIndex+7; @@ -402,7 +392,8 @@ template void addJointInfoFromConstraint(int linkIndex, if (con->m_typeConstraintData.m_name) { - info.m_jointName = strDup(con->m_typeConstraintData.m_name); + strcpy(info.m_jointName,con->m_typeConstraintData.m_name); + //info.m_linkName = strDup(con->m_typeConstraintData.m_name); } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 76d09dfe7..94ca9bea6 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -112,17 +112,6 @@ void PhysicsDirect::resetData() BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - for (int j = 0; jm_jointInfo.size(); j++) { - if (bodyJoints->m_jointInfo[j].m_jointName) - { - free(bodyJoints->m_jointInfo[j].m_jointName); - } - if (bodyJoints->m_jointInfo[j].m_linkName) - { - free(bodyJoints->m_jointInfo[j].m_linkName); - } - } delete (*bodyJointsPtr); } } @@ -1085,8 +1074,8 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - info.m_baseName = bodyJoints->m_baseName.c_str(); - info.m_bodyName = bodyJoints->m_bodyName.c_str(); + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); return true; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28b7ec5b1..3b2d152a7 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -214,8 +214,8 @@ enum b3JointInfoFlags struct b3JointInfo { - char* m_linkName; - char* m_jointName; + char m_linkName[1024]; + char m_jointName[1024]; int m_jointType; int m_qIndex; int m_uIndex; @@ -254,8 +254,8 @@ struct b3UserConstraint struct b3BodyInfo { - const char* m_baseName; - const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf + char m_baseName[1024]; + char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; struct b3DynamicsInfo diff --git a/examples/pybullet/unity3d/autogen/NativeMethods.cs b/examples/pybullet/unity3d/autogen/NativeMethods.cs index 29ec9f4f0..60511833b 100644 --- a/examples/pybullet/unity3d/autogen/NativeMethods.cs +++ b/examples/pybullet/unity3d/autogen/NativeMethods.cs @@ -425,13 +425,13 @@ public enum b3JointInfoFlags { [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] public struct b3JointInfo { - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_linkName; - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_jointName; /// int @@ -531,13 +531,13 @@ public struct b3UserConstraint { [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] public struct b3BodyInfo { - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_baseName; - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_bodyName; } @@ -624,8 +624,7 @@ public struct b3CameraImageData { public int m_pixelHeight; /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] - public string m_rgbColorData; + public System.IntPtr m_rgbColorData; /// float* public System.IntPtr m_depthValues; @@ -1444,7 +1443,7 @@ public static extern int b3GetBodyUniqueId(IntPtr physClient, int serialIndex) ///bodyUniqueId: int ///info: b3BodyInfo* [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint="b3GetBodyInfo")] -public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; + public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; /// Return Type: int diff --git a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs index 42bcfddf0..6007203da 100644 --- a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs +++ b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs @@ -10,29 +10,7 @@ using System; public class NewBehaviourScript : MonoBehaviour { - - - - [DllImport("TestCppPlug.dll")] - static extern int Add(int a, int b); - - [DllImport("TestCppPlug.dll")] - static extern int CallMe(Action action); - - [DllImport("TestCppPlug.dll")] - static extern IntPtr CreateSharedAPI(int id); - - [DllImport("TestCppPlug.dll")] - static extern int GetMyIdPlusTen(IntPtr api); - - [DllImport("TestCppPlug.dll")] - static extern void DeleteSharedAPI(IntPtr api); - - private void IWasCalled(int value) - { - text.text = value.ToString(); - } - + Text text; IntPtr sharedAPI; IntPtr pybullet; @@ -40,48 +18,76 @@ public class NewBehaviourScript : MonoBehaviour { // Use this for initialization void Start () { text = GetComponent(); - CallMe(IWasCalled); - sharedAPI = CreateSharedAPI(30); - pybullet = NativeMethods.b3ConnectPhysicsDirect();// SharedMemory(12347); + + + pybullet = NativeMethods.b3ConnectPhysicsDirect();// NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY); IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet); IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); int numBodies = NativeMethods.b3GetNumBodies(pybullet); cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf"); status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status); - if (statusType == EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED) + if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED) { numBodies = NativeMethods.b3GetNumBodies(pybullet); text.text = numBodies.ToString(); + cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, 0); + status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + statusType = (EnumSharedMemoryServerStatus) NativeMethods.b3GetStatusType(status); + + if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3VisualShapeInformation visuals = new b3VisualShapeInformation(); + NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals); + System.Console.WriteLine("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes); + System.IntPtr visualPtr = visuals.m_visualShapeData; + + for (int s = 0; s < visuals.m_numVisualShapes; s++) + { + b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData)); + visualPtr = new IntPtr(visualPtr.ToInt64()+(Marshal.SizeOf(typeof(b3VisualShapeData)))); + double x = visual.m_dimensions[0]; + double y = visual.m_dimensions[1]; + double z = visual.m_dimensions[2]; + System.Console.WriteLine("visual.m_visualGeometryType = " + visual.m_visualGeometryType); + System.Console.WriteLine("visual.m_dimensions" + x + "," + y + "," + z); + if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH) + { + System.Console.WriteLine("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName); + text.text = visual.m_meshAssetFileName; + } + } + } if (numBodies > 0) { - //b3BodyInfo info=new b3BodyInfo(); - //NativeMethods.b3GetBodyInfo(pybullet, 0, ref info ); - //text.text = info.m_baseName; + b3BodyInfo info=new b3BodyInfo(); + NativeMethods.b3GetBodyInfo(pybullet, 0, ref info ); + + text.text = info.m_baseName; } + + } - //IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr); } // Update is called once per frame void Update () { - //if (pybullet != IntPtr.Zero) + if (pybullet != IntPtr.Zero) { IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet); IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); - - //text.text = System.IO.Directory.GetCurrentDirectory().ToString(); - //text.text = Add(4, 5).ToString(); - //text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString(); } } void OnDestroy() { - NativeMethods.b3DisconnectSharedMemory(pybullet); + if (pybullet != IntPtr.Zero) + { + NativeMethods.b3DisconnectSharedMemory(pybullet); + } } }