sVrpn.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com
 
Copyright (c) 2013 I-maginer
 
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
 
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
 
-----------------------------------------------------------------------------
*/
 
 
 
#include <iostream>
#include "sVrpn.h"
 
 
extern int VRPN_ANALOG_CB;
extern int VRPN_BTN_CB;
extern int VRPN_BTN_STATE_CB;
extern int VRPN_TRACKER_CB;
extern int VRPN_TRACKER_VEL_CB;
extern int VRPN_TRACKER_ACC_CB;
extern int VRPN_DIAL_CB;
extern int VRPN_TEXT_CB;
 
SVrpn::SVrpn(const std::string& host, const std::string& name)
{
        m_host = host;
  m_name = name;
 
  m_analog = 0;
  m_button = 0;
  m_traker = 0;
  m_dial = 0;
  m_textMessage = 0;
 
  mEnableAnalogData = false;
  mEnableButtonData = false;
  mEnableButtonStateData = false;
  mEnableTrackerData = false;
  mEnableTrackerVelData = false;
  mEnableTrackerAccelData = false;
  mEnableDialData = false;
  mEnableTextData = false;
}
 
SVrpn::~SVrpn()
{
  SAFE_DELETE(m_analog);
  SAFE_DELETE(m_button);
  SAFE_DELETE(m_traker);
  SAFE_DELETE(m_dial);
  SAFE_DELETE(m_textMessage);
}
 
void SVrpn::EnableAnalogData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  
  mEnableAnalogData = state;
}
 
void SVrpn::EnableButtonData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableButtonData = state;
}
 
void SVrpn::EnableButtonStateData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableButtonStateData = state;
}
 
void SVrpn::EnableTrackerData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableTrackerData = state;
}
 
void SVrpn::EnableTrackerVelData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableTrackerVelData = state;
}
 
void SVrpn::EnableTrackerAccelData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableTrackerAccelData = state;
}
 
void SVrpn::EnableDialData(bool state) 
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableDialData = state;
}
 
void SVrpn::EnableTextData(bool state)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  mEnableTextData = state;
}
 
bool SVrpn::GetEnableAnalogData()
{
  return mEnableAnalogData;
}
 
bool SVrpn::GetEnableButtonData()
{
  return mEnableButtonData;
}
 
bool SVrpn::GetEnableButtonStateData()
{
  return mEnableButtonStateData;
}
 
bool SVrpn::GetEnableTrackerData()
{
  return mEnableTrackerData;
}
 
bool SVrpn::GetEnableTrackerVelData()
{
  return mEnableTrackerVelData;
}
 
bool SVrpn::GetEnableTrackerAccelData()
{
  return mEnableTrackerAccelData;
}
 
bool SVrpn::GetEnableDialData() 
{
  return mEnableDialData;
}
 
bool SVrpn::GetEnableTextData()
{
  return mEnableTextData;
}
 
void VRPN_CALLBACK SVrpn::handle_analog(void* userData, const vrpn_ANALOGCB analog)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableAnalogData())
  {
    SCbData* data = new SCbData();
    for (int i = 0; i < analog.num_channel; i++)
    {
      data->analogData.push_back((float)analog.channel[i]);
    }
 
    OBJpostEvent(VRPN_ANALOG_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_button_state(void* userData, const vrpn_BUTTONCB button)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableButtonStateData())
  {
    SCbData* data = new SCbData();
    data->valueId = button.button;
    data->btnState = button.state;
    OBJpostEvent(VRPN_BTN_STATE_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_button_states(void* userData, const vrpn_BUTTONSTATESCB button)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableButtonData())
  {
    SCbData* data = new SCbData();
    for (int i = 0; i < button.num_buttons; i++)
    {
      data->btnData.push_back(button.states[i]);
    }
 
    OBJpostEvent(VRPN_BTN_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_tracker(void* userData, const vrpn_TRACKERCB tracker)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableTrackerData())
  {
    SCbData* data = new SCbData();
    data->valueId = tracker.sensor;
    data->vectorData.push_back((float)tracker.pos[0]);
    data->vectorData.push_back((float)tracker.pos[1]);
    data->vectorData.push_back((float)tracker.pos[2]);
 
    data->quatData.push_back((float)tracker.quat[0]);
    data->quatData.push_back((float)tracker.quat[1]);
    data->quatData.push_back((float)tracker.quat[2]);
    data->quatData.push_back((float)tracker.quat[3]);
 
    OBJpostEvent(VRPN_TRACKER_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_tracker_vel(void* userData, const vrpn_TRACKERVELCB tracker)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableTrackerVelData())
  {
    SCbData* data = new SCbData();
    data->valueId = tracker.sensor;
    data->vectorData.push_back((float)tracker.vel[0]);
    data->vectorData.push_back((float)tracker.vel[1]);
    data->vectorData.push_back((float)tracker.vel[2]);
 
    data->quatData.push_back((float)tracker.vel_quat[0]);
    data->quatData.push_back((float)tracker.vel_quat[1]);
    data->quatData.push_back((float)tracker.vel_quat[2]);
    data->quatData.push_back((float)tracker.vel_quat[3]);
 
    OBJpostEvent(VRPN_TRACKER_VEL_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_tracker_acc(void* userData, const vrpn_TRACKERACCCB tracker)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableTrackerAccelData())
  {
    SCbData* data = new SCbData();
    data->valueId = tracker.sensor;
    data->vectorData.push_back((float)tracker.acc[0]);
    data->vectorData.push_back((float)tracker.acc[1]);
    data->vectorData.push_back((float)tracker.acc[2]);
 
    data->quatData.push_back((float)tracker.acc_quat[0]);
    data->quatData.push_back((float)tracker.acc_quat[1]);
    data->quatData.push_back((float)tracker.acc_quat[2]);
    data->quatData.push_back((float)tracker.acc_quat[3]);
 
    OBJpostEvent(VRPN_TRACKER_ACC_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void VRPN_CALLBACK SVrpn::handle_dial(void* userData, const vrpn_DIALCB dial)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableDialData())
  {
  }
}
 
void VRPN_CALLBACK SVrpn::handle_text(void* userData, const vrpn_TEXTCB text)
{
  SVrpn* svrpn = (SVrpn*)userData;
  if (svrpn->GetEnableTextData() && (text.type == vrpn_TEXT_NORMAL))
  {
    SCbData* data = new SCbData();
    data->textData = text.message;
 
    OBJpostEvent(VRPN_TEXT_CB, SCOL_PTR svrpn, SCOL_PTR data);
  }
}
 
void SVrpn::Update()
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
    
  // try to connect
  if (m_analog == 0 && m_button == 0 && m_traker == 0 && m_dial == 0 && m_textMessage == 0)
  {
    std::string deviceName = m_name + "@" + m_host;
    m_analog = new vrpn_Analog_Remote(deviceName.c_str());
    m_analog->register_change_handler(this, SVrpn::handle_analog);
 
    m_button = new vrpn_Button_Remote(deviceName.c_str());
    m_button->register_change_handler(this, SVrpn::handle_button_state);
          m_button->register_states_handler(this, SVrpn::handle_button_states);
 
    m_traker = new vrpn_Tracker_Remote(deviceName.c_str());
    m_traker->register_change_handler(this, SVrpn::handle_tracker);
          m_traker->register_change_handler(this, SVrpn::handle_tracker_vel);
          m_traker->register_change_handler(this, SVrpn::handle_tracker_acc);
 
    m_dial = new vrpn_Dial_Remote(deviceName.c_str());
    m_dial->register_change_handler(this, SVrpn::handle_dial);
 
    m_textMessage = new vrpn_Text_Receiver(deviceName.c_str());
    m_textMessage->register_message_handler(this, SVrpn::handle_text);
  }
  else // update
  {
    if (m_analog && mEnableAnalogData)
      m_analog->mainloop();
 
    if (m_button && (mEnableButtonData || mEnableButtonStateData))
      m_button->mainloop();
 
    if (m_traker && (mEnableTrackerData || mEnableTrackerAccelData || mEnableTrackerVelData))
      m_traker->mainloop();
 
    if (m_dial && mEnableDialData)
      m_dial->mainloop();
 
    if (m_textMessage && mEnableTextData)
      m_textMessage->mainloop();
  }
}
 
 
// Initialize singleton
sVrpnThread* sVrpnThread::_singleton = 0;
 
sVrpnThread::sVrpnThread()
{
  m_terminate = false;
  m_thread = boost::thread(boost::bind(&sVrpnThread::UpdateThread, this));
}
 
sVrpnThread::~sVrpnThread()
{
  m_terminate = true;
  m_thread.join();
}
 
sVrpnThread* sVrpnThread::GetInstance()
{
  if (0 == _singleton)
  {
    _singleton =  new sVrpnThread();
  }
 
  return _singleton;
}
 
void sVrpnThread::Kill()
{
  if (0 != _singleton)
  {
    delete _singleton;
    _singleton = 0;
  }
}
 
SVrpn* sVrpnThread::AddDevice(const std::string& host, const std::string& name)
{
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
  SVrpn* device = new SVrpn(host, name);
        deviceList.insert(device);
        return device;
}
 
void sVrpnThread::RemoveDevice(SVrpn* device)
{
  {
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
    DeviceList::iterator idevice = deviceList.find(device);
          if (idevice != deviceList.end())
          {
                  deviceList.erase(idevice);
                  SAFE_DELETE(device);
          }
  }
 
  if(deviceList.size() == 0)
  {
    Kill();
  }
}
 
void sVrpnThread::UpdateThread()
{
  while (!m_terminate)
  {
    {
#ifdef SCOL_STATIC
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
#else
  boost::mutex::scoped_lock l(m_mutex);
#endif
    
      DeviceList::iterator idevice;
            for (idevice = deviceList.begin(); idevice != deviceList.end(); idevice++)
            {
                    SVrpn* device = (*idevice);
        device->Update();
            }
    }
 
    // Sleep for 1ms so we don't eat the CPU
    vrpn_SleepMsecs(30);
  }
}