Commit 6e11e31e authored by Khalil Oumimoun's avatar Khalil Oumimoun
Browse files

VRPN support.

parent f421a854
###########################################################################
#
# 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)
/*=========================================================================
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
......@@ -93,7 +93,7 @@ public:
const Vec3d& getForce() const;
void setForce(Vec3d force);
protected:
public:
///
/// \brief Constructor
......
......@@ -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
......@@ -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
};
}
......@@ -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;
}
}
}
......
......@@ -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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment