VRPN support.

Examples/VRPN/CMakeLists.txt

+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0.txt
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+#
+###########################################################################
+
+project(Example-VRPN)
+
+#-----------------------------------------------------------------------------
+# Create executable
+#-----------------------------------------------------------------------------
+imstk_add_executable(${PROJECT_NAME} VRPNExample.cpp)
+
+#-----------------------------------------------------------------------------
+# Add the target to Examples folder
+#-----------------------------------------------------------------------------
+SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/VRPN)
+
+#-----------------------------------------------------------------------------
+# Link libraries to executable
+#-----------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)

Examples/VRPN/VRPNExample.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+	  http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+=========================================================================*/
+
+#include "imstkSimulationManager.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkAPIUtilities.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkCollisionGraph.h"
+#include "imstkCamera.h"
+#include "imstkLight.h"
+#include "imstkScene.h"
+
+//new
+#include "imstkMeshIO.h"
+#include "imstkCameraController.h"
+#include "imstkCube.h"
+#include "imstkSceneObjectController.h"
+
+//VRPN
+#include <imstkVRPNArduinoDeviceClient.h>
+#include <imstkVRPNDeviceClient.h>
+#include <imstkVRPNDeviceServer.h>
+
+using namespace imstk;
+
+///
+/// \brief This example demonstrates the data exchange between VRPN and imstk
+///
+int
+main()
+{
+	auto simManager = std::make_shared<SimulationManager>();
+	auto scene = simManager->createNewScene("VRPNExample");
+
+
+	//VRPN:
+ 
+	const std::string deviceName = "sa0";
+	/// \ IP address of the server.
+	const std::string serverIP = "";
+	/// \ the default VRPN port 3883
+	const int serverPort = 38833;
+
+
+	// Device Client
+	auto client = std::make_shared<VRPNDeviceClient>(deviceName);
+
+	//VRPN Server
+	auto server = std::make_shared<VRPNDeviceServer>(serverIP, serverPort);
+
+	//Device 1:
+	server->addDeviceClient(client, deviceName, imstk::DeviceType::Analog, 0);
+
+	//server->addDevice("Mouse0", imstk::DeviceType::Analog, 1);
+	simManager->addModule(server);
+
+	
+	// Load Mesh
+	auto mesh = MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.obj");
+	auto meshObject = std::make_shared<VisualObject>("meshObject");
+	meshObject->setVisualGeometry(mesh);
+	scene->addSceneObject(meshObject);
+
+	// Update Camera position
+	auto camera = scene->getCamera();
+	camera->setPosition(Vec3d(0, 0, 20));
+
+
+	auto camController = std::make_shared<CameraController>(camera, client);
+
+	//camController->setTranslationScaling(100);
+	//LOG(INFO) << camController->getTranslationOffset(); // should be the same than initial cam position
+	camController->setInversionFlags(CameraController::InvertFlag::rotY |
+		CameraController::InvertFlag::rotZ);
+
+	scene->addCameraController(camController);
+
+
+	// Light
+	auto light = std::make_shared<DirectionalLight>("light");
+	light->setFocalPoint(Vec3d(5, -8, -5));
+	light->setIntensity(1);
+	scene->addLight(light);
+
+	// Run
+	simManager->setActiveScene(scene);
+	simManager->start(SimulationStatus::Paused);
+	
+
+	return 0;
+}
\ No newline at end of file

Source/Devices/imstkDeviceClient.h

@@ -93,7 +93,7 @@ public:
     const Vec3d& getForce() const;
     void setForce(Vec3d force);
 
-protected:
+public:
 
     ///
     /// \brief Constructor

Source/Devices/imstkVRPNDeviceClient.cpp

@@ -29,30 +29,32 @@ void
 VRPNDeviceClient::initModule()
 {
     auto fullDeviceIp = this->getName().c_str();
-
-    m_vrpnTracker     = std::make_shared<vrpn_Tracker_Remote>(fullDeviceIp);
+	std::cout << "fullDeviceIP" << fullDeviceIp << std::endl;
+	// m_vrpnTracker     = std::make_shared<vrpn_Tracker_Remote>(fullDeviceIp);
     m_vrpnAnalog      = std::make_shared<vrpn_Analog_Remote>(fullDeviceIp);
     m_vrpnButton      = std::make_shared<vrpn_Button_Remote>(fullDeviceIp);
-    m_vrpnForceDevice = std::make_shared<vrpn_ForceDevice_Remote>(fullDeviceIp);
+  //  m_vrpnForceDevice = std::make_shared<vrpn_ForceDevice_Remote>(fullDeviceIp);
 
-    m_vrpnTracker->register_change_handler(this, trackerChangeHandler);
-    m_vrpnTracker->register_change_handler(this, velocityChangeHandler);
+   // m_vrpnTracker->register_change_handler(this, trackerChangeHandler);
+    //m_vrpnTracker->register_change_handler(this, velocityChangeHandler);
     m_vrpnAnalog->register_change_handler(this, analogChangeHandler);
     m_vrpnButton->register_change_handler(this, buttonChangeHandler);
-    m_vrpnForceDevice->register_force_change_handler(this, forceChangeHandler);
+    //m_vrpnForceDevice->register_force_change_handler(this, forceChangeHandler);
 
-    m_vrpnForceDevice->setFF_Origin(0, 0, 0);
-    m_vrpnForceDevice->setFF_Jacobian(0, 0, 0, 0, 0, 0, 0, 0, 0);
-    m_vrpnForceDevice->setFF_Radius(2);
+    //m_vrpnForceDevice->setFF_Origin(0, 0, 0);
+    //m_vrpnForceDevice->setFF_Jacobian(0, 0, 0, 0, 0, 0, 0, 0, 0);
+    //m_vrpnForceDevice->setFF_Radius(2);
 }
 
 void
 VRPNDeviceClient::runModule()
 {
+	/*
     if (this->getTrackingEnabled())
     {
-        m_vrpnTracker->mainloop();
+       m_vrpnTracker->mainloop();
     }
+	*/
     if (this->getAnalogicEnabled())
     {
         m_vrpnAnalog->mainloop();
@@ -63,42 +65,41 @@ VRPNDeviceClient::runModule()
     }
     if (this->getForceEnabled())
     {
-        m_vrpnForceDevice->setFF_Force(vrpn_float32(m_force[0]), vrpn_float32(m_force[1]), vrpn_float32(m_force[2]));
-        m_vrpnForceDevice->sendForceField();
-        m_vrpnForceDevice->mainloop();
+        //m_vrpnForceDevice->setFF_Force(vrpn_float32(m_force[0]), vrpn_float32(m_force[1]), vrpn_float32(m_force[2]));
+        //m_vrpnForceDevice->sendForceField();
+        //m_vrpnForceDevice->mainloop();
     }
 }
 
 void
 VRPNDeviceClient::cleanUpModule()
 {
-    m_vrpnTracker->unregister_change_handler(this, trackerChangeHandler);
-    m_vrpnTracker->unregister_change_handler(this, velocityChangeHandler);
+    //m_vrpnTracker->unregister_change_handler(this, trackerChangeHandler);
+    //m_vrpnTracker->unregister_change_handler(this, velocityChangeHandler);
     m_vrpnAnalog->unregister_change_handler(this, analogChangeHandler);
     m_vrpnButton->unregister_change_handler(this, buttonChangeHandler);
-    m_vrpnForceDevice->unregister_force_change_handler(this, forceChangeHandler);
+    //m_vrpnForceDevice->unregister_force_change_handler(this, forceChangeHandler);
 
-    m_vrpnForceDevice->stopForceField();
+    //m_vrpnForceDevice->stopForceField();
 
-    m_vrpnTracker.reset();
+    //m_vrpnTracker.reset();
     m_vrpnAnalog.reset();
     m_vrpnButton.reset();
-    m_vrpnForceDevice.reset();
+    //m_vrpnForceDevice.reset();
 }
 
 void VRPN_CALLBACK
 VRPNDeviceClient::trackerChangeHandler(void* userData, const _vrpn_TRACKERCB t)
 {
-    auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
-    deviceClient->m_position << t.pos[0], t.pos[1], t.pos[2];
+    //auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
+    //deviceClient->m_position << t.pos[0], t.pos[1], t.pos[2];
 
     Quatd quat;
     quat.x() = t.quat[0];
     quat.y() = t.quat[1];
     quat.z() = t.quat[2];
     quat.w() = t.quat[3];
-    deviceClient->m_orientation = quat;
-
+    //deviceClient->m_orientation = quat;
     //LOG(DEBUG) << "tracker: position = " << t.pos[0] << " " << t.pos[1] << " " << t.pos[2];
     //LOG(DEBUG) << "tracker: orientation = " << deviceClient->m_orientation.matrix();
 }
@@ -106,23 +107,20 @@ VRPNDeviceClient::trackerChangeHandler(void* userData, const _vrpn_TRACKERCB t)
 void VRPN_CALLBACK
 VRPNDeviceClient::analogChangeHandler(void* userData, const _vrpn_ANALOGCB a)
 {
-    auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
-
-    if (a.num_channel >= 3)
-    {
-        deviceClient->m_position << a.channel[0], a.channel[1], a.channel[2];
-        //LOG(DEBUG) << "analog: position = " << deviceClient->m_position;
-    }
-    if (a.num_channel >= 6)
-    {
-        deviceClient->m_orientation =
-            Rotd(a.channel[3] * PI, Vec3d::UnitX()) *
-            Rotd(a.channel[4] * PI, Vec3d::UnitY()) *
-            Rotd(a.channel[5] * PI, Vec3d::UnitZ());
-        //LOG(DEBUG) << "analog: orientation = " << deviceClient->m_orientation.matrix();
-    }
+	auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
+	//deviceClient->m_position << channel.a[0], channel.a[1], channel.a[2];
+	int nbChannels = a.num_channel;
+
+	std::cout << "Analog : ";
+	
+	for (int i = 0; i < a.num_channel; i++)
+	{
+		std::cout << a.channel[i] << " ";
+	}
+	std::cout << std::endl;
 }
 
+/*
 void VRPN_CALLBACK
 VRPNDeviceClient::velocityChangeHandler(void* userData, const _vrpn_TRACKERVELCB v)
 {
@@ -130,15 +128,17 @@ VRPNDeviceClient::velocityChangeHandler(void* userData, const _vrpn_TRACKERVELCB
     deviceClient->m_velocity << v.vel[0], v.vel[1], v.vel[2];
     //LOG(DEBUG) << "tracker: velocity = " << deviceClient->m_velocity;
 }
-
+*/
 void VRPN_CALLBACK
 VRPNDeviceClient::buttonChangeHandler(void* userData, const _vrpn_BUTTONCB b)
 {
-    auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
-    deviceClient->m_buttons[b.button] = (b.state == 1);
+    //auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
+    //deviceClient->m_buttons[b.button] = (b.state == 1);
     //LOG(DEBUG) << "buttons: " << b.button << " = " << deviceClient->m_buttons[b.button];
+    std::cout << "buttons: " << b.button << " STATE: " << b.state << std::endl;
+	//std::cout << deviceClient->m_buttons[0] << std::endl;
 }
-
+/*
 void VRPN_CALLBACK
 VRPNDeviceClient::forceChangeHandler(void* userData, const _vrpn_FORCECB f)
 {
@@ -146,4 +146,5 @@ VRPNDeviceClient::forceChangeHandler(void* userData, const _vrpn_FORCECB f)
     deviceClient->m_force << f.force[0], f.force[1], f.force[2];
     LOG(DEBUG) << "forceDevice: force = " << deviceClient->m_force;
 }
+*/
 } // imstk

Source/Devices/imstkVRPNDeviceClient.h

@@ -44,18 +44,27 @@ public:
     ///
     /// \brief Constructor
     ///
+	/*
     VRPNDeviceClient(const std::string& deviceName, const std::string& ip) :
         DeviceClient(deviceName, ip),
         Module(deviceName + "@" + ip)
     {}
+	*/
+	
+	explicit VRPNDeviceClient(const std::string& deviceName) :
+		DeviceClient(deviceName, "localhost"),
+		Module(deviceName + "@" + "localhost")
+	{}
 
     ///
     /// \brief Destructor
     ///
+
     virtual ~VRPNDeviceClient()
     {}
 
 protected:
+	friend class VRPNDeviceServer;
     ///
     /// \brief Initialize device client module
     ///
@@ -71,7 +80,7 @@ protected:
     ///
     void cleanUpModule() override;
 
-private:
+public:
 
     ///
     /// \brief VRPN call back for position and orientation data
@@ -98,7 +107,7 @@ private:
     /// \param v VRPN callback structure containing new position and
     /// orientation data
     ///
-    static void VRPN_CALLBACK velocityChangeHandler(void* userData, const _vrpn_TRACKERVELCB v);
+    //static void VRPN_CALLBACK velocityChangeHandler(void* userData, const _vrpn_TRACKERVELCB v);
 
     ///
     /// \brief VRPN call back for button changed (pressed or released)
@@ -114,11 +123,11 @@ private:
     /// internal data
     /// \param f VRPN callback structure containing new force data
     ///
-    static void VRPN_CALLBACK forceChangeHandler(void* userData, const _vrpn_FORCECB f);
+    //static void VRPN_CALLBACK forceChangeHandler(void* userData, const _vrpn_FORCECB f);
 
     std::shared_ptr<vrpn_Tracker_Remote>     m_vrpnTracker;     //!< VRPN position/orientation interface
     std::shared_ptr<vrpn_Analog_Remote>      m_vrpnAnalog;      //!< VRPN position/orientation interface
     std::shared_ptr<vrpn_Button_Remote>      m_vrpnButton;      //!< VRPN button interface
-    std::shared_ptr<vrpn_ForceDevice_Remote> m_vrpnForceDevice; //!< VRPN force interface
+    //std::shared_ptr<vrpn_ForceDevice_Remote> m_vrpnForceDevice; //!< VRPN force interface
 };
 }

Source/Devices/imstkVRPNDeviceServer.cpp

@@ -18,7 +18,7 @@
    limitations under the License.
 
 =========================================================================*/
-
+//#pragma comment( lib, "Devices" )
 #include "imstkVRPNDeviceServer.h"
 
 #include "vrpn_3DConnexion.h"
@@ -30,9 +30,15 @@
 #endif
 
 #include "imstkLogger.h"
+#include <map>
+#include <string>
+#include <stdexcept>
+#include "imstkVRPNDeviceClient.h"
 
 namespace imstk
 {
+	/*
+
 void
 VRPNDeviceServer::addDevice(const std::string& deviceName, DeviceType deviceType, int id)
 {
@@ -45,6 +51,22 @@ VRPNDeviceServer::addDevice(const std::string& deviceName, DeviceType deviceType
                      << "Use HDAPIDeviceClient instead of VRPNDeviceServer/Client for ";
     }
 }
+*/
+
+void
+VRPNDeviceServer::addDeviceClient(std::shared_ptr<VRPNDeviceClient> client, const std::string& deviceName, DeviceType deviceType, int id)
+	{
+		//m_deviceInfoMap[deviceName] = std::make_pair(deviceType, id);
+		m_deviceClients2.push_back(client);
+		m_deviceInfoMap[deviceName] = std::make_pair(deviceType, id);
+
+		if (deviceType == DeviceType::PhantomOmni)
+		{
+			LOG(WARNING) << "VRPNDeviceServer::addDevice warning: OpenHaptics support on VRPN "
+				<< "currently unstable for the Phantom Omni (no force feedback implemented).\n"
+				<< "Use HDAPIDeviceClient instead of VRPNDeviceServer/Client for ";
+		}
+	}
 
 void
 VRPNDeviceServer::addSerialDevice(const std::string& deviceName, DeviceType deviceType, const std::string& port, int baudRate, int id)
@@ -60,8 +82,10 @@ void
 VRPNDeviceServer::initModule()
 {
     std::string ip = m_machine + ":" + std::to_string(m_port);
+
     m_serverConnection = vrpn_create_server_connection(ip.c_str());
 
+
     m_deviceConnections = new vrpn_MainloopContainer();
 
     for (const auto& device : m_deviceInfoMap)
@@ -70,51 +94,53 @@ VRPNDeviceServer::initModule()
         DeviceType  type = device.second.first;
         auto        id   = device.second.second;
 
+		std::string address = name + "@" + m_machine;
+		const char* _address = address.c_str();
+
         switch (type)
         {
-        case DeviceType::SpaceExplorer3DConnexion:
-        {
-            m_deviceConnections->add(new vrpn_3DConnexion_SpaceExplorer(name.c_str(), m_serverConnection));
-        } break;
-        case DeviceType::Navigator3DConnexion:
+        case DeviceType::Tracker:
         {
-            m_deviceConnections->add(new vrpn_3DConnexion_Navigator(name.c_str(), m_serverConnection));
+			SerialInfo connectionSettings = m_SerialInfoMap[name];
+            //m_deviceConnections->add(new vrpn_3DConnexion_SpaceExplorer(name.c_str(), m_serverConnection));
+			//vrpn_Tracker_Remote* vrpnTracker = new vrpn_Tracker_Remote(_address);
+			//m_deviceConnections->add(vrpnTracker);
+			//vrpnTracker->register_change_handler(0, VRPNDeviceClient::trackerChangeHandler);
+			
         } break;
-        case DeviceType::NovintFalcon:
+        case DeviceType::Button:
         {
-#ifdef VRPN_USE_LIBNIFALCON
-            m_deviceConnections->add(new vrpn_Tracker_NovintFalcon(name.c_str(), m_serverConnection,
-                                                                   id, "4-button", "stamper"));
-#else
-            LOG(WARNING) << "VRPNDeviceServer::initModule error: no support for Novint Falcon in VRPN. "
-                         << "Build VRPN with VRPN_USE_LIBNIFALCON.";
-#endif
+			vrpn_Button_Remote* vrpnButton = new vrpn_Button_Remote(_address);
+			m_deviceConnections->add(vrpnButton);
+
+			vrpnButton->register_change_handler(0, VRPNDeviceClient::buttonChangeHandler);
+
         } break;
         case DeviceType::PhantomOmni:
         {
 #ifdef VRPN_USE_PHANTOM_SERVER
             char* deviceName = const_cast<char*>(name.c_str());
-            m_deviceConnections->add(new vrpn_Phantom(deviceName, m_serverConnection, 90.0f, deviceName));
+            //m_deviceConnections->add(new vrpn_Phantom(deviceName, m_serverConnection, 90.0f, deviceName));
 #else
             LOG(WARNING) << "VRPNDeviceServer::initModule error: no support for Phantom Omni in VRPN. "
                          << "Install OpenHaptics SDK, the omni driver, and build VRPN with VRPN_USE_PHANTOM_SERVER.";
 #endif
         } break;
-        //case DeviceType::OSVR_HDK:
-        //{
-        //    m_deviceConnections->add(new vrpn_Tracker_OSVRHackerDevKit(name.c_str(), m_serverConnection));
-        //} break;
-        case DeviceType::Arduino:
+
+        case DeviceType::Analog:
         {
             SerialInfo connectionSettings = m_SerialInfoMap[name];
-            //open with 6 channels (max needed for IMU, can use less)
-            m_deviceConnections->add(new vrpn_Streaming_Arduino(name.c_str(), m_serverConnection, connectionSettings.port, 6, connectionSettings.baudRate));
+			vrpn_Analog_Remote* vrpnAnalog = new vrpn_Analog_Remote(_address);
+			m_deviceConnections->add(vrpnAnalog);
+
+			vrpnAnalog->register_change_handler(0, VRPNDeviceClient::analogChangeHandler);
+
         } break;
         default:
         {
             LOG(WARNING) << "VRPNDeviceServer::initModule error: can not connect to "
                          << name << ", device type unknown.";
-        } break;
+        } break;	
         }
     }
 }

Source/Devices/imstkVRPNDeviceServer.h

@@ -31,20 +31,19 @@
 
 // imstk
 #include "imstkModule.h"
+#include "imstkVRPNDeviceClient.h"
 
 namespace imstk
 {
 ///
-/// \brief Enumeration for device types
+/// \brief Enumeration for device types - type of communication data
 ///
 enum class DeviceType
 {
-    SpaceExplorer3DConnexion,
-    Navigator3DConnexion,
-    NovintFalcon,
-    PhantomOmni,
-    OSVR_HDK,
-    Arduino
+    Analog,
+	Button,
+	Tracker,
+	PhantomOmni
 };
 
 ///
@@ -72,7 +71,8 @@ public:
     ///
     /// \brief Add device
     ///
-    void addDevice(const std::string& deviceName, DeviceType deviceType, int id = 0);
+    //void addDevice(const std::string& deviceName, DeviceType deviceType, int id = 0);
+	void addDeviceClient(std::shared_ptr<VRPNDeviceClient> client, const std::string& deviceName, DeviceType deviceType, int id = 0);
 
     ///
     /// \brief Add serial device
@@ -111,5 +111,7 @@ private:
     std::map<std::string, SerialInfo> m_SerialInfoMap;
     vrpn_Connection* m_serverConnection = nullptr;                     ///< VRPN server connection
     vrpn_MainloopContainer* m_deviceConnections = nullptr;             ///< VRPN device connections
+
+	std::vector<std::shared_ptr<VRPNDeviceClient>> m_deviceClients2; ///< list of OpenHaptics
 };
 } // imstk