ENH: Implement imstkVRPNDeviceClient

Base/Devices/CMakeLists.txt

+#-----------------------------------------------------------------------------
+# Create target
+#-----------------------------------------------------------------------------
+include(imstkAddLibrary)
+imstk_add_library( Devices
+  DEPENDS
+    Core
+    VRPN
+  )
+
+#-----------------------------------------------------------------------------
+# Testing
+#-----------------------------------------------------------------------------
+if( iMSTK_BUILD_TESTING )
+  add_subdirectory( Testing )
+endif()

Base/Devices/imstkDeviceClient.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 "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;
+}
+
+}

Base/Devices/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.
+
+   =========================================================================*/
+
+#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

Base/Devices/imstkVRPNDeviceClient.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 "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;
+}
+}

Base/Devices/imstkVRPNDeviceClient.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.
+
+   =========================================================================*/
+
+#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

Base/SimulationManager/CMakeLists.txt

@@ -5,6 +5,7 @@ include(imstkAddLibrary)
 imstk_add_library( SimulationManager
   DEPENDS
     Rendering
+    Devices
   )
   
 #-----------------------------------------------------------------------------

CMake/External/External_VRPN.cmake

+#-----------------------------------------------------------------------------
+# 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
+  )

CMake/FindVRPN.cmake

+#-----------------------------------------------------------------------------
+# 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}")
+