An update will be applied December 9th, between 12PM and 1:00PM EST (UTC -5:00). The site may be slow during that time.

Commit dedf109b authored by Alexis Girault's avatar Alexis Girault
Browse files

Merge 'external-devices-sprint' into refactoring

parents 508eab36 9db036d3
......@@ -35,11 +35,24 @@ void
stdSink::ReceiveLogMessage(g3::LogMessageMover logEntry)
{
auto level = logEntry.get()._level;
auto color = GetColor(level);
auto message = logEntry.get().message();
#ifndef WIN32
auto color = GetColor(level);
std::cout << "\033[" << color << "m"
<< logEntry.get().message()
<< "\033[m" << std::endl;
<< message
<< "\033[m" << std::endl;
#else
if (level.value == WARNING.value || level.value == FATAL.value)
{
std::cerr << message << std::endl;
}
else
{
std::cout << message << std::endl;
}
#endif
}
void
......
......@@ -28,7 +28,6 @@
namespace imstk {
// 2D vector
using Vec2f = Eigen::Vector2f;
using Vec2d = Eigen::Vector2d;
......@@ -49,6 +48,10 @@ using VecNd = Eigen::VectorXd;
using Quatf = Eigen::Quaternionf;
using Quatd = Eigen::Quaterniond;
// Angle-Axis
using Rotf = Eigen::AngleAxisf;
using Rotd = Eigen::AngleAxisd;
// 3x3 Matrix
using Mat3f = Eigen::Matrix3f;
using Mat3d = Eigen::Matrix3d;
......
#-----------------------------------------------------------------------------
# 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 std::string&
DeviceClient::getIp()
{
return m_ip;
}
void
DeviceClient::setIp(const std::string& ip)
{
m_ip = ip;
}
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 {
///
/// \class DeviceClient
/// \brief Base class for any device client
///
class DeviceClient : public Module
{
public:
virtual ~DeviceClient() {}
// Accessors
///
/// \brief Get/Set the device IP
///
const std::string& getIp();
void setIp(const std::string& ip);
///
/// \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 ip):
Module(name),
m_ip(ip)
{}
std::string m_ip; //!< Connection device IP
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 fullDeviceIp = this->getName() + "@" + this->getIp();
m_vrpnTracker = std::make_shared<vrpn_Tracker_Remote>(fullDeviceIp.c_str());
m_vrpnAnalog = std::make_shared<vrpn_Analog_Remote>(fullDeviceIp.c_str());
m_vrpnButton = std::make_shared<vrpn_Button_Remote>(fullDeviceIp.c_str());
m_vrpnForceDevice = std::make_shared<vrpn_ForceDevice_Remote>(fullDeviceIp.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 = " << t.pos[0] << " " << t.pos[1] << " " << t.pos[2];
//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 =
Rotd(a.channel[3]*M_PI,Vec3d::UnitX())*
Rotd(a.channel[4]*M_PI,Vec3d::UnitY())*
Rotd(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 <memory>
#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 ip):
DeviceClient(name, ip)
{}
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
/*=========================================================================
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.
=========================================================================*/