Commit 94d54543 authored by Alexis Girault's avatar Alexis Girault

ENH: Implement imstkVRPNDeviceClient

parent e8e8b047
#-----------------------------------------------------------------------------
# Create target
#-----------------------------------------------------------------------------
include(imstkAddLibrary)
imstk_add_library( Devices
DEPENDS
Core
VRPN
)
#-----------------------------------------------------------------------------
# Testing
#-----------------------------------------------------------------------------
if( iMSTK_BUILD_TESTING )
add_subdirectory( Testing )
endif()
/*=========================================================================
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 "imstkDeviceClient.h"
#include "g3log/g3log.hpp"
namespace imstk {
const DeviceType&
DeviceClient::getType()
{
return m_type;
}
void
DeviceClient::setType(const DeviceType& type)
{
m_type = type;
}
const std::string&
DeviceClient::getUrl()
{
return m_url;
}
void
DeviceClient::setUrl(const std::string& url)
{
m_url = url;
}
const bool&
DeviceClient::getTrackingEnabled() const
{
return m_trackingEnabled;
}
void
DeviceClient::setTrackingEnabled(const bool& status)
{
if (this->getStatus() != ModuleStatus::INACTIVE)
{
LOG(WARNING) << "DeviceClient::setTrackingEnabled error: can not change listeners for"
<< this->getName() << " while the device is active.";
return;
}
m_trackingEnabled = status;
}
const bool&
DeviceClient::getAnalogicEnabled() const
{
return m_analogicEnabled;
}
void
DeviceClient::setAnalogicEnabled(const bool& status)
{
if (this->getStatus() != ModuleStatus::INACTIVE)
{
LOG(WARNING) << "DeviceClient::setAnalogicEnabled error: can not change listeners for"
<< this->getName() << " while the device is active.";
return;
}
m_analogicEnabled = status;
}
const bool&
DeviceClient::getButtonsEnabled() const
{
return m_buttonsEnabled;
}
void
DeviceClient::setButtonsEnabled(const bool& status)
{
if (this->getStatus() != ModuleStatus::INACTIVE)
{
LOG(WARNING) << "DeviceClient::setButtonsEnabled error: can not change listeners for"
<< this->getName() << " while the device is active.";
return;
}
m_buttonsEnabled = status;
}
const bool&
DeviceClient::getForceEnabled() const
{
return m_forceEnabled;
}
void
DeviceClient::setForceEnabled(const bool& status)
{
if (this->getStatus() != ModuleStatus::INACTIVE)
{
LOG(WARNING) << "DeviceClient::setForceEnabled error: can not change listeners for"
<< this->getName() << " while the device is active.";
return;
}
m_forceEnabled = status;
}
const Vec3d&
DeviceClient::getPosition() const
{
return m_position;
}
const Vec3d&
DeviceClient::getVelocity() const
{
return m_velocity;
}
const Quatd&
DeviceClient::getOrientation() const
{
return m_orientation;
}
const std::map<size_t, bool>&
DeviceClient::getButtons() const
{
return m_buttons;
}
bool
DeviceClient::getButton(size_t buttonId) const
{
if (m_buttons.find(buttonId) == m_buttons.end())
{
LOG(WARNING) << "DeviceClient::getButton warning: button "
<< buttonId << " was not found in the buttons list.";
return false;
}
return m_buttons.at(buttonId);
}
const Vec3d&
DeviceClient::getForce() const
{
return m_force;
}
const Vec3d&
DeviceClient::getTorque() const
{
return m_torque;
}
}
/*=========================================================================
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.
=========================================================================*/
#ifndef imstkDeviceClient_h
#define imstkDeviceClient_h
#include <map>
#include "imstkModule.h"
#include "imstkMath.h"
namespace imstk {
enum class DeviceType
{
SPACE_EXPLORER_3DCONNEXION,
NAVIGATOR_3DCONNEXION
};
///
/// \class DeviceClient
/// \brief Base class for any device client
///
class DeviceClient : public Module
{
public:
virtual ~DeviceClient() {}
// Accessors
///
/// \brief Get/Set the type of the device
///
const DeviceType& getType();
void setType(const DeviceType& type);
///
/// \brief Get/Set the device URL
///
const std::string& getUrl();
void setUrl(const std::string& url);
///
/// \brief Get/Set what listeners to enable on the device: tracking, analogic, force, buttons.
///
const bool& getTrackingEnabled() const;
void setTrackingEnabled(const bool& status);
const bool& getAnalogicEnabled() const;
void setAnalogicEnabled(const bool& status);
const bool& getButtonsEnabled() const;
void setButtonsEnabled(const bool& status);
const bool& getForceEnabled() const;
void setForceEnabled(const bool& status);
///
/// \brief Get the device position
///
const Vec3d& getPosition() const;
///
/// \brief Get the device velocity
///
const Vec3d& getVelocity() const;
///
/// \brief Get the device orientation
///
const Quatd& getOrientation() const;
///
/// \brief Get the status of the device buttons
///
const std::map<size_t, bool>& getButtons() const;
///
/// \brief Get the status of a device button
///
bool getButton(size_t buttonId) const;
///
/// \brief Get the device force
///
const Vec3d& getForce() const;
///
/// \brief Get the device torque
///
const Vec3d& getTorque() const;
protected:
DeviceClient(std::string name, std::string url, DeviceType type):
Module(name),
m_url(url),
m_type(type)
{}
DeviceType m_type; //!< Device type
std::string m_url; //!< Connection device URL
bool m_trackingEnabled = true; //!< Tracking enabled if true
bool m_analogicEnabled = true; //!< Analogic enabled if true
bool m_buttonsEnabled = true; //!< Buttons enabled if true
bool m_forceEnabled = false; //!< Force enabled if true
Vec3d m_position = Vec3d::Zero(); //!< Position of end effector
Vec3d m_velocity = Vec3d::Zero(); //!< Linear velocity of end effector
Quatd m_orientation = Quatd::Identity(); //!< Orientation of the end effector
std::map<size_t, bool> m_buttons; //!< Buttons: true = pressed/false = not pressed
Vec3d m_force = Vec3d::Zero(); //!< Force vector
Vec3d m_torque = Vec3d::Zero(); //!< Torque vector
};
}
#endif // ifndef imstkDeviceClient_h
/*=========================================================================
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 "imstkVRPNDeviceClient.h"
#include "g3log/g3log.hpp"
namespace imstk {
void
VRPNDeviceClient::initModule()
{
std::string fullDeviceUrl = this->getName() + "@" + this->getUrl();
m_vrpnTracker = std::make_shared<vrpn_Tracker_Remote>(fullDeviceUrl.c_str());
m_vrpnAnalog = std::make_shared<vrpn_Analog_Remote>(fullDeviceUrl.c_str());
m_vrpnButton = std::make_shared<vrpn_Button_Remote>(fullDeviceUrl.c_str());
m_vrpnForceDevice = std::make_shared<vrpn_ForceDevice_Remote>(fullDeviceUrl.c_str());
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);
}
void
VRPNDeviceClient::runModule()
{
if (this->getTrackingEnabled())
{
m_vrpnTracker->mainloop();
}
if (this->getAnalogicEnabled())
{
m_vrpnAnalog->mainloop();
}
if (this->getButtonsEnabled())
{
m_vrpnButton->mainloop();
}
if (this->getForceEnabled())
{
m_vrpnForceDevice->mainloop();
}
}
void
VRPNDeviceClient::cleanUpModule()
{
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_vrpnTracker.reset();
m_vrpnAnalog.reset();
m_vrpnButton.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];
deviceClient->m_orientation.x() = t.quat[0];
deviceClient->m_orientation.y() = t.quat[1];
deviceClient->m_orientation.z() = t.quat[2];
deviceClient->m_orientation.w() = t.quat[3];
LOG(DEBUG) << "tracker: position = " << deviceClient->m_position;
LOG(DEBUG) << "tracker: orientation = " << deviceClient->m_orientation.matrix();
}
void VRPN_CALLBACK
VRPNDeviceClient::analogChangeHandler(void *userData, const _vrpn_ANALOGCB a)
{
auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
if (a.num_channel > 0)
{
deviceClient->m_position << a.channel[0], a.channel[1], a.channel[2];
LOG(DEBUG) << "analog: position = " << deviceClient->m_position;
}
if (a.num_channel > 3)
{
deviceClient->m_orientation =
Eigen::AngleAxisd(a.channel[3]*M_PI,Vec3d::UnitX())*
Eigen::AngleAxisd(a.channel[4]*M_PI,Vec3d::UnitY())*
Eigen::AngleAxisd(a.channel[5]*M_PI,Vec3d::UnitZ());
LOG(DEBUG) << "analog: orientation = " << deviceClient->m_orientation.matrix();
}
}
void VRPN_CALLBACK
VRPNDeviceClient::velocityChangeHandler(void *userData, const _vrpn_TRACKERVELCB v)
{
auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
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);
LOG(DEBUG) << "buttons: " << b.button << " = " << deviceClient->m_buttons[b.button];
}
void VRPN_CALLBACK
VRPNDeviceClient::forceChangeHandler(void *userData, const _vrpn_FORCECB f)
{
auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
deviceClient->m_force << f.force[0], f.force[1], f.force[2];
LOG(DEBUG) << "forceDevice: force = " << deviceClient->m_force;
}
}
/*=========================================================================
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.
=========================================================================*/
#ifndef imstkVRPNDeviceClient_h
#define imstkVRPNDeviceClient_h
#include <map>
#include "imstkDeviceClient.h"
#include <vrpn_Configure.h>
#include <vrpn_Tracker.h>
#include <vrpn_Analog.h>
#include <vrpn_Button.h>
#include <vrpn_ForceDevice.h>
namespace imstk {
///
/// \class VRPNDeviceClient
/// \brief Subclass of DeviceClient using VRPN
///
class VRPNDeviceClient : public DeviceClient
{
public:
VRPNDeviceClient(std::string name, std::string url, DeviceType type):
DeviceClient(name, url, type)
{}
virtual ~VRPNDeviceClient() {}
protected:
void initModule() override;
void runModule() override;
void cleanUpModule() override;
private:
///
/// \brief VRPN call back for position and orientation data
/// \param userData Pointer to this to allow updating
/// internal data
/// \param b VRPN callback structure containing new position and
/// orientation data
///
static void VRPN_CALLBACK trackerChangeHandler(void *userData, const _vrpn_TRACKERCB t);
///
/// \brief VRPN call back for position and orientation data
/// \param userData Pointer to this to allow updating
/// internal data
/// \param b VRPN callback structure containing new position and
/// orientation data
///
static void VRPN_CALLBACK analogChangeHandler(void *userData, const _vrpn_ANALOGCB a);
///
/// \brief VRPN call back for velocity data
/// \param userData Pointer to this to allow updating
/// internal data
/// \param v VRPN callback structure containing new position and
/// orientation data
///
static void VRPN_CALLBACK velocityChangeHandler(void *userData, const _vrpn_TRACKERVELCB v);
///
/// \brief VRPN call back for button changed (pressed or released)
/// \param userData Pointer to this to allow updating
/// internal data
/// \param b VRPN callback structure containing new button data
///
static void VRPN_CALLBACK buttonChangeHandler(void *userData, const _vrpn_BUTTONCB b);
///
/// \brief VRPN call back for force data
/// \param userData Pointer to this to allow updating
/// internal data
/// \param f VRPN callback structure containing new force data
///
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
};
}
#endif // ifndef imstkVRPNDeviceClient_h
......@@ -5,6 +5,7 @@ include(imstkAddLibrary)
imstk_add_library( SimulationManager
DEPENDS
Rendering
Devices
)
#-----------------------------------------------------------------------------
......
#-----------------------------------------------------------------------------
# Add External Project
#-----------------------------------------------------------------------------
include(imstkAddExternalProject)
imstk_add_external_project( VRPN
REPOSITORY https://github.com/vrpn/vrpn.git
GIT_TAG bc1788410478f4fbedab479524537f2524ca9262
CMAKE_ARGS
-DBUILD_TESTING:BOOL=OFF
-DVRPN_SUBPROJECT_BUILD:BOOL=ON
-DVRPN_BUILD_CLIENTS:BOOL=OFF
-DVRPN_BUILD_SERVERS:BOOL=OFF
-DVRPN_BUILD_CLIENT_LIBRARY:BOOL=ON
-DVRPN_BUILD_SERVER_LIBRARY:BOOL=ON
-DVRPN_INSTALL:BOOL=OFF
-DVRPN_BUILD_PYTHON:BOOL=OFF
-DVRPN_USE_GPM_MOUSE:BOOL=OFF
-DVRPN_USE_HID:BOOL=ON
#-DVRPN_USE_LIBNIFALCON:BOOL=ON
#DEPENDENCIES "LibNiFalcon"
#RELATIVE_INCLUDE_PATH ""
#VERBOSE
)
#-----------------------------------------------------------------------------
# Find path
#-----------------------------------------------------------------------------
find_path(VRPN_INCLUDE_DIR
NAMES
vrpn_Configure.h
)
mark_as_advanced(VRPN_INCLUDE_DIR)
set(VRPN_INCLUDE_DIRS "${VRPN_INCLUDE_DIR}")
list(APPEND VRPN_INCLUDE_DIRS "${VRPN_INCLUDE_DIR}/quat")
list(APPEND VRPN_INCLUDE_DIRS "${VRPN_INCLUDE_DIR}/atmellib")
list(APPEND VRPN_INCLUDE_DIRS "${VRPN_INCLUDE_DIR}/server_src")
list(APPEND VRPN_INCLUDE_DIRS "${VRPN_INCLUDE_DIR}/client_src")
message(STATUS "VRPN_INCLUDE_DIRS : ${VRPN_INCLUDE_DIRS}")
#-----------------------------------------------------------------------------
# Find library
#-----------------------------------------------------------------------------
find_library(VRPN_LIBRARY
NAMES
vrpn
vrpnb
)
mark_as_advanced(VRPN_LIBRARY)
find_library(QUAT_LIBRARY
NAMES
quat
quatd
)
mark_as_advanced(QUAT_LIBRARY)
set(VRPN_LIBRARIES ${VRPN_LIBRARY} ${QUAT_LIBRARY})
message(STATUS "VRPN_LIBRARIES : ${VRPN_LIBRARIES}")
#-----------------------------------------------------------------------------
# Find package
#-----------------------------------------------------------------------------
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(VRPN
REQUIRED_VARS
VRPN_INCLUDE_DIRS
VRPN_LIBRARIES)
#-----------------------------------------------------------------------------
# If missing target, create it
#-----------------------------------------------------------------------------
if(VRPN_FOUND AND NOT TARGET VRPN)
add_library(VRPN INTERFACE IMPORTED)
set_target_properties(VRPN PROPERTIES
INTERFACE_LINK_LIBRARIES "${VRPN_LIBRARIES}"
INTERFACE_INCLUDE_DIRECTORIES "${VRPN_INCLUDE_DIRS}"
)
endif()
......@@ -83,6 +83,8 @@ imstk_define_dependency(g3log)
imstk_define_dependency(Eigen)
imstk_define_dependency(VegaFEM)
imstk_define_dependency(VTK)
imstk_define_dependency(VRPN)
#imstk_define_dependency(LibNiFalcon)
#-----------------------------------------------------------------------------
# SUPERBUILD
......@@ -122,11 +124,16 @@ find_package( VegaFEM REQUIRED CONFIG )
find_package( VTK REQUIRED )
include( ${VTK_USE_FILE} )
# VRPN
find_package( VRPN REQUIRED )
include_directories( ${VRPN_INCLUDE_DIRS} )
#--------------------------------------------------------------------------
# Add Source code subdirectories
#--------------------------------------------------------------------------
add_subdirectory(Base/Core)
add_subdirectory(Base/Geometry)
add_subdirectory(Base/Devices)
add_subdirectory(Base/Scene)
add_subdirectory(Base/Rendering)
add_subdirectory(Base/SimulationManager)
......
......@@ -23,10 +23,14 @@
#include "imstkIsometricMap.h"
#include "imstkOneToOneMap.h"
// Devices
#include "imstkVRPNDeviceClient.h"
#include "g3log/g3log.hpp"
void testViewer();
void testDeviceClient();
void testReadMesh();
void testViewer();
void testAnalyticalGeometry();
void testScenesManagement();
void testIsometricMap();
......@@ -41,8 +45,9 @@ int main()
<< "Starting Sandbox\n"
<< "****************\n";
testDeviceClient();
//testViewer();
testReadMesh();
//testReadMesh();
//testAnalyticalGeometry();
//testScenesManagement();
//testIsometricMap();
......@@ -54,6 +59,17 @@ int main()
return 0;
}
void testDeviceClient()
{
auto sdk = std::make_shared<imstk::SimulationManager>();
auto client = std::make_shared<imstk::VRPNDeviceClient>("device0",
"localhost",
imstk::DeviceType::SPACE_EXPLORER_3DCONNEXION);
client->setLoopDelay(1000);
client->start();
}
void testReadMesh()
{
// SDK and Scene
......
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