From b572fe43f98237e9d734dd72bbb941f1f10b018a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 5 Oct 2017 12:59:58 -0700 Subject: [PATCH] fix signal handling in ExampleBrowser on linux/mac on termination expose all analogue axes from OpenVR (5 controllers, each x,y -> 10 floats) in pybullet.getVREvents(allAnalogAxes=1) --- .../CommonInterfaces/CommonExampleInterface.h | 2 +- examples/ExampleBrowser/main.cpp | 13 +++----- .../PhysicsServerCommandProcessor.cpp | 12 ++++++- .../SharedMemory/PhysicsServerExample.cpp | 9 ++++-- examples/SharedMemory/SharedMemoryPublic.h | 3 +- .../StandaloneMain/hellovr_opengl_main.cpp | 32 +++++++------------ examples/pybullet/examples/vrEvent.py | 28 ++++++++++++++-- examples/pybullet/pybullet.c | 21 ++++++++++-- 8 files changed, 81 insertions(+), 39 deletions(-) diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index 220f9a583..0bccb2dd5 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -55,7 +55,7 @@ public: virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; virtual bool keyboardCallback(int key, int state)=0; - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {} + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]) {} virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){} virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]){} virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]){} diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 07c168309..3847181b5 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -30,19 +30,16 @@ static OpenGLExampleBrowser* sExampleBrowser=0; static void cleanup(int signo) { - if (interrupted) { // this is the second time, we're hanging somewhere - // if (example) { - // example->abort(); - // } - + if (!interrupted) { // this is the second time, we're hanging somewhere b3Printf("Aborting and deleting SharedMemoryCommon object"); - sleep(1); delete sExampleBrowser; - sExampleBrowser = 0; + sleep(1); + sExampleBrowser = 0; errx(EXIT_FAILURE, "aborted example on signal %d", signo); } else { - b3Printf("no action"); + b3Printf("no action"); + exit(EXIT_FAILURE); } interrupted = true; warnx("caught signal %d", signo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index db744441f..4cc70b724 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -889,8 +889,18 @@ struct b3VRControllerEvents if (vrEvents[i].m_numMoveEvents) { m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis; + for (int a=0;a<10;a++) + { + m_vrEvents[controlledId].m_auxAnalogAxis[a] = vrEvents[i].m_auxAnalogAxis[a]; + } + } else + { + m_vrEvents[controlledId].m_analogAxis = 0; + for (int a=0;a<10;a++) + { + m_vrEvents[controlledId].m_auxAnalogAxis[a] = 0; + } } - if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents) { m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 208eeafe4..3cf461ae5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1330,7 +1330,7 @@ public: btVector3 getRayTo(int x,int y); virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]); - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis); + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]); virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]); virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]); @@ -2874,7 +2874,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt } -void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) +void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10]) { if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS) @@ -2927,6 +2927,11 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3]; m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++; m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis; + for (int i=0;i<10;i++) + { + m_args[0].m_vrControllerEvents[controllerId].m_auxAnalogAxis[i] = auxAnalogAxes[i]; + } + m_args[0].m_csGUI->unlock(); } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 0d766fc64..3a02af8d4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -339,6 +339,7 @@ enum b3VREventType VR_GENERIC_TRACKER_MOVE_EVENT=8, }; +#define MAX_VR_ANALOG_AXIS 5 #define MAX_VR_BUTTONS 64 #define MAX_VR_CONTROLLERS 8 @@ -380,7 +381,7 @@ struct b3VRControllerEvent float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT - + float m_auxAnalogAxis[MAX_VR_ANALOG_AXIS*2];//store x,y per axis, only valid if VR_CONTROLLER_MOVE_EVENT int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo }; diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 3a98e6a87..76517c2e6 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -814,7 +814,15 @@ bool CMainApplication::HandleInput() for (int button = 0; button < vr::k_EButton_Max; button++) { uint64_t trigger = vr::ButtonMaskFromId((vr::EVRButtonId)button); - + + btAssert(vr::k_unControllerStateAxisCount>=5); + float allAxis[10];//store x,y times 5 controllers + int index=0; + for (int i=0;i<5;i++) + { + allAxis[index++]=state.rAxis[i].x; + allAxis[index++]=state.rAxis[i].y; + } bool isTrigger = (state.ulButtonPressed&trigger) != 0; if (isTrigger) { @@ -824,31 +832,15 @@ bool CMainApplication::HandleInput() if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0) { // printf("Device PRESSED: %d, button %d\n", unDevice, button); - if (button==2) - { - //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - - ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync - ///so it can (and likely will) cause crashes - ///add a special debug drawer that deals with this - //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ - //btIDebugDraw::DBG_DrawConstraintLimits+ - //btIDebugDraw::DBG_DrawConstraints - //; - //gDebugDrawFlags = btIDebugDraw::DBG_DrawFrames; - - - - } - sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); } else { + // printf("Device MOVED: %d\n", unDevice); - sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x, allAxis); } } else @@ -871,7 +863,7 @@ bool CMainApplication::HandleInput() } else { - sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x,allAxis); } } } diff --git a/examples/pybullet/examples/vrEvent.py b/examples/pybullet/examples/vrEvent.py index beb369b1d..790d6881b 100644 --- a/examples/pybullet/examples/vrEvent.py +++ b/examples/pybullet/examples/vrEvent.py @@ -8,7 +8,9 @@ import pybullet as p CONTROLLER_ID = 0 POSITION=1 ORIENTATION=2 +NUM_MOVE_EVENTS=5 BUTTONS=6 +ANALOG_AXIS=8 #assume that the VR physics server is already started before c = p.connect(p.SHARED_MEMORY) @@ -20,7 +22,7 @@ p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") -prevPosition=[None]*p.VR_MAX_CONTROLLERS +prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS widths = [3]*p.VR_MAX_CONTROLLERS @@ -32,10 +34,27 @@ colors[3] = [0,0,0.5] colors[4] = [0.5,0.5,0.] colors[5] = [.5,.5,.5] -while True: +controllerId = -1 +pt=[0,0,0] + +print("waiting for VR controller trigger") +while (controllerId<0): events = p.getVREvents() + for e in (events): + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + +print("Using controllerId="+str(controllerId)) + +while True: + events = p.getVREvents(allAnalogAxes=1) for e in (events): + if (e[CONTROLLER_ID]==controllerId ): + for a in range(10): + print("analog axis"+str(a)+"="+str(e[8][a])) if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED): prevPosition[e[CONTROLLER_ID]] = e[POSITION] if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED): @@ -51,7 +70,10 @@ while True: pt = prevPosition[e[CONTROLLER_ID]] #print(prevPosition[e[0]]) - #print(e[1]) + print("e[POSITION]") + print(e[POSITION]) + print("pt") + print(pt) diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]] lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2] ptDistThreshold = 0.01 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 302e4a5e7..e2622db3f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4277,9 +4277,10 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* int statusType; int deviceTypeFilter = VR_DEVICE_CONTROLLER; int physicsClientId = 0; + int allAnalogAxes = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"deviceTypeFilter", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &deviceTypeFilter, &physicsClientId)) + static char* kwlist[] = {"deviceTypeFilter", "allAnalogAxes", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, &deviceTypeFilter, &allAnalogAxes, &physicsClientId)) { return NULL; } @@ -4307,7 +4308,8 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents); for (i = 0; i < vrEvents.m_numControllerEvents; i++) { - PyObject* vrEventObj = PyTuple_New(8); + int numFields = allAnalogAxes? 9 : 8; + PyObject* vrEventObj = PyTuple_New(numFields); PyTuple_SetItem(vrEventObj, 0, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId)); { @@ -4340,6 +4342,19 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* PyTuple_SetItem(vrEventObj, 6, buttonsObj); } PyTuple_SetItem(vrEventObj, 7, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_deviceType)); + + if (allAnalogAxes) + { + PyObject* buttonsObj = PyTuple_New(MAX_VR_ANALOG_AXIS*2); + int b; + for (b = 0; b < MAX_VR_ANALOG_AXIS*2; b++) + { + PyObject* axisVal = PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_auxAnalogAxis[b]); + PyTuple_SetItem(buttonsObj, b, axisVal); + } + PyTuple_SetItem(vrEventObj, 8, buttonsObj); + } + PyTuple_SetItem(vrEventsObj, i, vrEventObj); } return vrEventsObj;