<commit_before>///////////////////////////////////////////////////////////////////////////////
// FILE:          PrecisExcite.cpp
// PROJECT:       Micro-Manager
// SUBSYSTEM:     DeviceAdapters
//-----------------------------------------------------------------------------
// DESCRIPTION:   PrecisExcite controller adapter
// COPYRIGHT:     University of California, San Francisco, 2009
//
// AUTHOR:        Arthur Edelstein, arthuredelstein@gmail.com, 3/17/2009
//
// LICENSE:       This file is distributed under the BSD license.
//                License text is included with the source distribution.
//
//                This file 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.
//
//                IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//                CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//                INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES.
//
// CVS:
//

#ifdef WIN32
   #include <windows.h>
   #define snprintf _snprintf
#endif

#include "../../MMDevice/MMDevice.h"
#include "PrecisExcite.h"
#include <string>
#include <math.h>
#include "../../MMDevice/ModuleInterface.h"
#include "../../MMDevice/DeviceUtils.h"
#include <sstream>

// Controller
const char* g_ControllerName = "PrecisExcite";
const char* g_Keyword_Intensity = "Intensity";
const char* g_Keyword_Trigger = "Trigger";
const char* g_Keyword_Trigger_Sequence = "TriggerSequence";
const char * carriage_return = "\r";
const char * line_feed = "\n";

///////////////////////////////////////////////////////////////////////////////
// Exported MMDevice API
///////////////////////////////////////////////////////////////////////////////
MODULE_API void InitializeModuleData()
{
   RegisterDevice(g_ControllerName, MM::ShutterDevice, "PrecisExcite LED illuminator");
}

MODULE_API MM::Device* CreateDevice(const char* deviceName)
{
   if (deviceName == 0)
      return 0;

   if (strcmp(deviceName, g_ControllerName) == 0)
   {
      // create Controller
      Controller* pController = new Controller(g_ControllerName);
      return pController;
   }

   return 0;
}

MODULE_API void DeleteDevice(MM::Device* pDevice)
{
   delete pDevice;
}

///////////////////////////////////////////////////////////////////////////////
// Controller implementation
// ~~~~~~~~~~~~~~~~~~~~

Controller::Controller(const char* name) :
   initialized_(false),
   intensity_(0),
   state_(0),
   name_(name),
   busy_(false),
   error_(0),
   changedTime_(0.0)
{
   assert(strlen(name) < (unsigned int) MM::MaxStrLength);

   InitializeDefaultErrorMessages();

   // create pre-initialization properties
   // ------------------------------------

   // Name
   CreateProperty(MM::g_Keyword_Name, name_.c_str(), MM::String, true);

   // Description
   CreateProperty(MM::g_Keyword_Description, "PrecisExcite LED Illuminator", MM::String, true);

   // Port
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnPort);
   CreateProperty(MM::g_Keyword_Port, "Undefined", MM::String, false, pAct, true);

   EnableDelay(); // signals that the delay setting will be used
   UpdateStatus();
}

Controller::~Controller()
{
   Shutdown();
}

bool Controller::Busy()
{
   MM::MMTime interval = GetCurrentMMTime() - changedTime_;
   MM::MMTime delay(GetDelayMs()*1000.0);
   if (interval < delay)
      return true;
   else
      return false;
}

void Controller::GetName(char* name) const
{
   assert(name_.length() < CDeviceUtils::GetMaxStringLength());
   CDeviceUtils::CopyLimitedString(name, name_.c_str());
}

int Controller::Initialize()
{

   // set property list
   // -----------------
   /*
   MM::Device* serialDevice = GetCoreCallback()->GetDevice(this, port_.c_str());
   if (serialDevice == NULL) {
      LogMessage("Serial port not found");
      return DEVICE_SERIAL_COMMAND_FAILED;
   }
*/
   this->LogMessage("Controller::Initialize()");

   currentChannel_ = 0;

   ReadGreeting();
   int result = ReadChannelLabels();
   if (result != DEVICE_OK)
	   return result;

   GenerateChannelChooser();
   GeneratePropertyIntensity();
   GeneratePropertyState();
   GeneratePropertyTrigger();
   GeneratePropertyTriggerSequence();

   initialized_ = true;
   return HandleErrors();

}

void Controller::ReadGreeting()
{
   do {
      ReceiveOneLine();
   } while (! buf_string_.empty());
}

int Controller::ReadChannelLabels()
{
   buf_tokens_.clear();
   string label;

   Purge();
   Send("LAMS");
   do {
      ReceiveOneLine();
      buf_tokens_.push_back(buf_string_);
   }
      while(! buf_string_.empty());

   for (unsigned int i=0;i<buf_tokens_.size();i++)
   {
      if (buf_tokens_[i].substr(0,3).compare("LAM")==0) {
         string label = buf_tokens_[i].substr(6);
         StripString(label);

         //This skips invalid channels
         if (label.compare("----") == 0)
            continue;

         channelLetters_.push_back(buf_tokens_[i][4]); // Read 4th character
         // This is a temporary log entry to debug an issue with channel labels
         // that appear to contain an invalid character at the end.
         std::ostringstream ss;
         ss << "debug: last char of stripped label is: \'" <<
            static_cast<int>(label[label.size()]) << "\' (as decimal int)";
         LogMessage(ss.str().c_str(), true);

         channelLabels_.push_back(label);
      }
   }

   if (channelLabels_.size() == 0)
	   return DEVICE_ERR;
   else
	   return DEVICE_OK;
}

/////////////////////////////////////////////
// Property Generators
/////////////////////////////////////////////

void Controller::GeneratePropertyState()
{
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnState);
   CreateProperty(MM::g_Keyword_State, "0", MM::Integer, false, pAct);
   AddAllowedValue(MM::g_Keyword_State, "0");
   AddAllowedValue(MM::g_Keyword_State, "1");
}

void Controller::GeneratePropertyTrigger()
{
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnTrigger);
   CreateProperty("Trigger", "Off", MM::String, false, pAct);
   for (TriggerType i=OFF;i<=FOLLOW_PULSE;i=TriggerType(i+1))
      AddAllowedValue("Trigger", TriggerLabels[i].c_str());
   SetProperty("Trigger","Off");
}

void Controller::GenerateChannelChooser()
{
   if (! channelLabels_.empty()) {
      CPropertyAction* pAct;
      pAct = new CPropertyAction (this, &Controller::OnChannelLabel);
      CreateProperty("ChannelLabel", channelLabels_[0].c_str(), MM::String, false, pAct);

      SetAllowedValues("ChannelLabel",channelLabels_);
      SetProperty("ChannelLabel",channelLabels_[0].c_str());

   }
}

void Controller::GeneratePropertyIntensity()
{
   string intensityName;
   CPropertyActionEx* pAct;
   for (unsigned i=0;i<channelLetters_.size();i++)
   {
      pAct = new CPropertyActionEx(this, &Controller::OnIntensity, i);
      intensityName = g_Keyword_Intensity;
      intensityName.push_back(channelLetters_[i]);
      CreateProperty(intensityName.c_str(), "0", MM::Integer, false, pAct);
      SetPropertyLimits(intensityName.c_str(), 0, 100);
   }
}

int Controller::Shutdown()
{
   if (initialized_)
   {
      initialized_ = false;
   }
   return HandleErrors();
}

void Controller::GeneratePropertyTriggerSequence()
{
   int ret;
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnTriggerSequence);
   ret = CreateProperty(g_Keyword_Trigger_Sequence, "ABCD0", MM::String, false, pAct);
   SetProperty(g_Keyword_Trigger_Sequence, "ABCD0");
}

///////////////////////////////////////////////////////////////////////////////
// String utilities
///////////////////////////////////////////////////////////////////////////////

void Controller::StripString(string& StringToModify)
{
   if(StringToModify.empty()) return;

   const char* spaces = " \f\n\r\t\v";
   size_t startIndex = StringToModify.find_first_not_of(spaces);
   size_t endIndex = StringToModify.find_last_not_of(spaces);
   string tempString = StringToModify;
   StringToModify.erase();

   StringToModify = tempString.substr(startIndex, (endIndex-startIndex+ 1) );
}

///////////////////////////////////////////////////////////////////////////////
// Action handlers
///////////////////////////////////////////////////////////////////////////////

int Controller::OnPort(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(port_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      if (initialized_)
      {
         // revert
         pProp->Set(port_.c_str());
         return ERR_PORT_CHANGE_FORBIDDEN;
      }

      pProp->Get(port_);
   }

   return HandleErrors();
}

int Controller::OnIntensity(MM::PropertyBase* pProp, MM::ActionType eAct, long index)
{

   long intensity;
   if (eAct == MM::BeforeGet)
   {
      GetIntensity(intensity,index);
      pProp->Set(intensity);
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(intensity);
      SetIntensity(intensity, index);
   }

   return HandleErrors();
}

int Controller::OnChannelLabel(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(currentChannelLabel_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      GetState(state_);
      pProp->Get(currentChannelLabel_);
      for (unsigned int i=0;i<channelLabels_.size();i++)
         if (channelLabels_[i].compare(currentChannelLabel_) == 0)
         {
            currentChannel_ = i;
            SetState(state_);
         }

   }

   return HandleErrors();
}

int Controller::OnState(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      GetState(state_);
      pProp->Set(state_);
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(state_);
      SetState(state_);
   }

   return HandleErrors();
}

int Controller::OnTrigger(MM::PropertyBase* pProp, MM::ActionType eAct)
{

   if (eAct == MM::BeforeGet)
   {
      pProp->Set(trigger_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      char cmd=0;
      pProp->Get(trigger_);
      for (int i=0;i<5;i++)
      {
         if (trigger_.compare(TriggerLabels[i])==0)
         {
            cmd = TriggerCmd[i];
            triggerMode_ = (TriggerType) i;
         }
      }

      SetTrigger();

   }
   return HandleErrors();
}

int Controller::OnTriggerSequence(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(triggerSequence_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(triggerSequence_);
      SetTrigger();
   }
   return HandleErrors();
}

///////////////////////////////////////////////////////////////////////////////
// Utility methods
///////////////////////////////////////////////////////////////////////////////

void Controller::SetTrigger()
{
   stringstream msg;
   msg << "SQX" << carriage_return;

   for (unsigned int i=0;i<triggerSequence_.size();i++)
   {
      msg << "SQ" << triggerSequence_[i] << carriage_return;
   }

   triggerMessage_ = msg.str();

   Illuminate();

}

void Controller::Illuminate()
{
   stringstream msg;
   if (state_==0)
   {
      if (triggerMode_ == OFF || triggerMode_ == FOLLOW_PULSE)
         msg << "SQX" << carriage_return << "C" << channelLetters_[currentChannel_] << "F" << carriage_return << "AZ";
      else
         msg << "SQX" << "AZ";
   }
   else if (state_==1)
   {
      if (triggerMode_ == OFF) {
         msg << "SQZ" << carriage_return;
         for (unsigned int i=0; i<channelLetters_.size(); i++) {
            msg << "C" << channelLetters_[i];
            if (i == currentChannel_)
               msg << "N";
            else
               msg << "F";
            msg << carriage_return;
         }
      }
      else if (triggerMode_ == FOLLOW_PULSE)
         msg << "SQZ" << carriage_return << "A" << channelLetters_[currentChannel_] << "#";
      else
         msg << triggerMessage_ << "SQ" << TriggerCmd[triggerMode_];
   }

   Send(msg.str());
}

void Controller::SetIntensity(long intensity, long index)
{
   stringstream msg;
   msg << "C" << channelLetters_[index] << "I" << intensity;
   Purge();
   Send(msg.str());
   ReceiveOneLine();

}

void Controller::GetIntensity(long& intensity, long index)
{
   stringstream msg;
   string ans;
   msg << "C" << channelLetters_[index] << "?";
   Purge();
   Send(msg.str());
   ReceiveOneLine();

   if (! buf_string_.empty())
      if (0 == buf_string_.compare(0,2,msg.str(),0,2))
      {
         intensity = atol(buf_string_.substr(2,3).c_str());
      }

}

void Controller::SetState(long state)
{
   state_ = state;
   stringstream msg;
   Illuminate();

   // Set timer for the Busy signal
   changedTime_ = GetCurrentMMTime();
}

void Controller::GetState(long &state)
{
   if (triggerMode_ == OFF) {
      Purge();
      Send("C?");
      long stateTmp = 0;

      for (unsigned int i=0;i<channelLetters_.size();i++)
      {
         ReceiveOneLine();

         if (! buf_string_.empty())
            if (buf_string_[5]=='N')
               stateTmp = 1;
      }
      state = stateTmp;
   }
   else
      state = state_;

}

int Controller::HandleErrors()
{
   int lastError = error_;
   error_ = 0;
   return lastError;
}

/////////////////////////////////////
//  Communications
/////////////////////////////////////

void Controller::Send(string cmd)
{
   int ret = SendSerialCommand(port_.c_str(), cmd.c_str(), carriage_return);
   if (ret!=DEVICE_OK)
      error_ = DEVICE_SERIAL_COMMAND_FAILED;
}

void Controller::ReceiveOneLine()
{
   buf_string_ = "";
   GetSerialAnswer(port_.c_str(), line_feed, buf_string_);

}

void Controller::Purge()
{
   int ret = PurgeComPort(port_.c_str());
   if (ret!=0)
      error_ = DEVICE_SERIAL_COMMAND_FAILED;
}

//********************
// Shutter API
//********************

int Controller::SetOpen(bool open)
{
   SetState((long) open);
   return HandleErrors();
}

int Controller::GetOpen(bool& open)
{
   long state;
   GetState(state);
   if (state==1)
      open = true;
   else if (state==0)
      open = false;
   else
      error_ = DEVICE_UNKNOWN_POSITION;

   return HandleErrors();
}

int Controller::Fire(double deltaT)
{
   deltaT=0; // Suppress warning
   error_ = DEVICE_UNSUPPORTED_COMMAND;
   return HandleErrors();
}
<commit_msg>PrecisExcite: Made invalid channel detection more robust (Egor Zindy)<commit_after>///////////////////////////////////////////////////////////////////////////////
// FILE:          PrecisExcite.cpp
// PROJECT:       Micro-Manager
// SUBSYSTEM:     DeviceAdapters
//-----------------------------------------------------------------------------
// DESCRIPTION:   PrecisExcite controller adapter
// COPYRIGHT:     University of California, San Francisco, 2009
//
// AUTHOR:        Arthur Edelstein, arthuredelstein@gmail.com, 3/17/2009
//
// LICENSE:       This file is distributed under the BSD license.
//                License text is included with the source distribution.
//
//                This file 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.
//
//                IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//                CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//                INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES.
//
// CVS:
//

#ifdef WIN32
   #include <windows.h>
   #define snprintf _snprintf
#endif

#include "../../MMDevice/MMDevice.h"
#include "PrecisExcite.h"
#include <string>
#include <math.h>
#include "../../MMDevice/ModuleInterface.h"
#include "../../MMDevice/DeviceUtils.h"
#include <sstream>

// Controller
const char* g_ControllerName = "PrecisExcite";
const char* g_Keyword_Intensity = "Intensity";
const char* g_Keyword_Trigger = "Trigger";
const char* g_Keyword_Trigger_Sequence = "TriggerSequence";
const char * carriage_return = "\r";
const char * line_feed = "\n";

///////////////////////////////////////////////////////////////////////////////
// Exported MMDevice API
///////////////////////////////////////////////////////////////////////////////
MODULE_API void InitializeModuleData()
{
   RegisterDevice(g_ControllerName, MM::ShutterDevice, "PrecisExcite LED illuminator");
}

MODULE_API MM::Device* CreateDevice(const char* deviceName)
{
   if (deviceName == 0)
      return 0;

   if (strcmp(deviceName, g_ControllerName) == 0)
   {
      // create Controller
      Controller* pController = new Controller(g_ControllerName);
      return pController;
   }

   return 0;
}

MODULE_API void DeleteDevice(MM::Device* pDevice)
{
   delete pDevice;
}

///////////////////////////////////////////////////////////////////////////////
// Controller implementation
// ~~~~~~~~~~~~~~~~~~~~

Controller::Controller(const char* name) :
   initialized_(false),
   intensity_(0),
   state_(0),
   name_(name),
   busy_(false),
   error_(0),
   changedTime_(0.0)
{
   assert(strlen(name) < (unsigned int) MM::MaxStrLength);

   InitializeDefaultErrorMessages();

   // create pre-initialization properties
   // ------------------------------------

   // Name
   CreateProperty(MM::g_Keyword_Name, name_.c_str(), MM::String, true);

   // Description
   CreateProperty(MM::g_Keyword_Description, "PrecisExcite LED Illuminator", MM::String, true);

   // Port
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnPort);
   CreateProperty(MM::g_Keyword_Port, "Undefined", MM::String, false, pAct, true);

   EnableDelay(); // signals that the delay setting will be used
   UpdateStatus();
}

Controller::~Controller()
{
   Shutdown();
}

bool Controller::Busy()
{
   MM::MMTime interval = GetCurrentMMTime() - changedTime_;
   MM::MMTime delay(GetDelayMs()*1000.0);
   if (interval < delay)
      return true;
   else
      return false;
}

void Controller::GetName(char* name) const
{
   assert(name_.length() < CDeviceUtils::GetMaxStringLength());
   CDeviceUtils::CopyLimitedString(name, name_.c_str());
}

int Controller::Initialize()
{

   // set property list
   // -----------------
   /*
   MM::Device* serialDevice = GetCoreCallback()->GetDevice(this, port_.c_str());
   if (serialDevice == NULL) {
      LogMessage("Serial port not found");
      return DEVICE_SERIAL_COMMAND_FAILED;
   }
*/
   this->LogMessage("Controller::Initialize()");

   currentChannel_ = 0;

   ReadGreeting();
   int result = ReadChannelLabels();
   if (result != DEVICE_OK)
	   return result;

   GenerateChannelChooser();
   GeneratePropertyIntensity();
   GeneratePropertyState();
   GeneratePropertyTrigger();
   GeneratePropertyTriggerSequence();

   initialized_ = true;
   return HandleErrors();

}

void Controller::ReadGreeting()
{
   do {
      ReceiveOneLine();
   } while (! buf_string_.empty());
}

int Controller::ReadChannelLabels()
{
   buf_tokens_.clear();
   string label;

   Purge();
   Send("LAMS");
   do {
      ReceiveOneLine();
      buf_tokens_.push_back(buf_string_);
   }
      while(! buf_string_.empty());

   for (unsigned int i=0;i<buf_tokens_.size();i++)
   {
      if (buf_tokens_[i].substr(0,3).compare("LAM")==0) {
         string label = buf_tokens_[i].substr(6);
         StripString(label);

         //This skips invalid channels.
	 //Invalid names seem to have a different number of dashes.
	 //pe2: First invalid is called ----, then second is -----
         if (label.substr(0,4).compare("----") == 0)
            continue;

         channelLetters_.push_back(buf_tokens_[i][4]); // Read 4th character
         // This is a temporary log entry to debug an issue with channel labels
         // that appear to contain an invalid character at the end.
         std::ostringstream ss;
         ss << "debug: last char of stripped label is: \'" <<
            static_cast<int>(label[label.size()]) << "\' (as decimal int)";
         LogMessage(ss.str().c_str(), true);

         channelLabels_.push_back(label);
      }
   }

   if (channelLabels_.size() == 0)
	   return DEVICE_ERR;
   else
	   return DEVICE_OK;
}

/////////////////////////////////////////////
// Property Generators
/////////////////////////////////////////////

void Controller::GeneratePropertyState()
{
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnState);
   CreateProperty(MM::g_Keyword_State, "0", MM::Integer, false, pAct);
   AddAllowedValue(MM::g_Keyword_State, "0");
   AddAllowedValue(MM::g_Keyword_State, "1");
}

void Controller::GeneratePropertyTrigger()
{
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnTrigger);
   CreateProperty("Trigger", "Off", MM::String, false, pAct);
   for (TriggerType i=OFF;i<=FOLLOW_PULSE;i=TriggerType(i+1))
      AddAllowedValue("Trigger", TriggerLabels[i].c_str());
   SetProperty("Trigger","Off");
}

void Controller::GenerateChannelChooser()
{
   if (! channelLabels_.empty()) {
      CPropertyAction* pAct;
      pAct = new CPropertyAction (this, &Controller::OnChannelLabel);
      CreateProperty("ChannelLabel", channelLabels_[0].c_str(), MM::String, false, pAct);

      SetAllowedValues("ChannelLabel",channelLabels_);
      SetProperty("ChannelLabel",channelLabels_[0].c_str());

   }
}

void Controller::GeneratePropertyIntensity()
{
   string intensityName;
   CPropertyActionEx* pAct;
   for (unsigned i=0;i<channelLetters_.size();i++)
   {
      pAct = new CPropertyActionEx(this, &Controller::OnIntensity, i);
      intensityName = g_Keyword_Intensity;
      intensityName.push_back(channelLetters_[i]);
      CreateProperty(intensityName.c_str(), "0", MM::Integer, false, pAct);
      SetPropertyLimits(intensityName.c_str(), 0, 100);
   }
}

int Controller::Shutdown()
{
   if (initialized_)
   {
      initialized_ = false;
   }
   return HandleErrors();
}

void Controller::GeneratePropertyTriggerSequence()
{
   int ret;
   CPropertyAction* pAct = new CPropertyAction (this, &Controller::OnTriggerSequence);
   ret = CreateProperty(g_Keyword_Trigger_Sequence, "ABCD0", MM::String, false, pAct);
   SetProperty(g_Keyword_Trigger_Sequence, "ABCD0");
}

///////////////////////////////////////////////////////////////////////////////
// String utilities
///////////////////////////////////////////////////////////////////////////////

void Controller::StripString(string& StringToModify)
{
   if(StringToModify.empty()) return;

   const char* spaces = " \f\n\r\t\v";
   size_t startIndex = StringToModify.find_first_not_of(spaces);
   size_t endIndex = StringToModify.find_last_not_of(spaces);
   string tempString = StringToModify;
   StringToModify.erase();

   StringToModify = tempString.substr(startIndex, (endIndex-startIndex+ 1) );
}

///////////////////////////////////////////////////////////////////////////////
// Action handlers
///////////////////////////////////////////////////////////////////////////////

int Controller::OnPort(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(port_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      if (initialized_)
      {
         // revert
         pProp->Set(port_.c_str());
         return ERR_PORT_CHANGE_FORBIDDEN;
      }

      pProp->Get(port_);
   }

   return HandleErrors();
}

int Controller::OnIntensity(MM::PropertyBase* pProp, MM::ActionType eAct, long index)
{

   long intensity;
   if (eAct == MM::BeforeGet)
   {
      GetIntensity(intensity,index);
      pProp->Set(intensity);
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(intensity);
      SetIntensity(intensity, index);
   }

   return HandleErrors();
}

int Controller::OnChannelLabel(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(currentChannelLabel_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      GetState(state_);
      pProp->Get(currentChannelLabel_);
      for (unsigned int i=0;i<channelLabels_.size();i++)
         if (channelLabels_[i].compare(currentChannelLabel_) == 0)
         {
            currentChannel_ = i;
            SetState(state_);
         }

   }

   return HandleErrors();
}

int Controller::OnState(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      GetState(state_);
      pProp->Set(state_);
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(state_);
      SetState(state_);
   }

   return HandleErrors();
}

int Controller::OnTrigger(MM::PropertyBase* pProp, MM::ActionType eAct)
{

   if (eAct == MM::BeforeGet)
   {
      pProp->Set(trigger_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      char cmd=0;
      pProp->Get(trigger_);
      for (int i=0;i<5;i++)
      {
         if (trigger_.compare(TriggerLabels[i])==0)
         {
            cmd = TriggerCmd[i];
            triggerMode_ = (TriggerType) i;
         }
      }

      SetTrigger();

   }
   return HandleErrors();
}

int Controller::OnTriggerSequence(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(triggerSequence_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(triggerSequence_);
      SetTrigger();
   }
   return HandleErrors();
}

///////////////////////////////////////////////////////////////////////////////
// Utility methods
///////////////////////////////////////////////////////////////////////////////

void Controller::SetTrigger()
{
   stringstream msg;
   msg << "SQX" << carriage_return;

   for (unsigned int i=0;i<triggerSequence_.size();i++)
   {
      msg << "SQ" << triggerSequence_[i] << carriage_return;
   }

   triggerMessage_ = msg.str();

   Illuminate();

}

void Controller::Illuminate()
{
   stringstream msg;
   if (state_==0)
   {
      if (triggerMode_ == OFF || triggerMode_ == FOLLOW_PULSE)
         msg << "SQX" << carriage_return << "C" << channelLetters_[currentChannel_] << "F" << carriage_return << "AZ";
      else
         msg << "SQX" << "AZ";
   }
   else if (state_==1)
   {
      if (triggerMode_ == OFF) {
         msg << "SQZ" << carriage_return;
         for (unsigned int i=0; i<channelLetters_.size(); i++) {
            msg << "C" << channelLetters_[i];
            if (i == currentChannel_)
               msg << "N";
            else
               msg << "F";
            msg << carriage_return;
         }
      }
      else if (triggerMode_ == FOLLOW_PULSE)
         msg << "SQZ" << carriage_return << "A" << channelLetters_[currentChannel_] << "#";
      else
         msg << triggerMessage_ << "SQ" << TriggerCmd[triggerMode_];
   }

   Send(msg.str());
}

void Controller::SetIntensity(long intensity, long index)
{
   stringstream msg;
   msg << "C" << channelLetters_[index] << "I" << intensity;
   Purge();
   Send(msg.str());
   ReceiveOneLine();

}

void Controller::GetIntensity(long& intensity, long index)
{
   stringstream msg;
   string ans;
   msg << "C" << channelLetters_[index] << "?";
   Purge();
   Send(msg.str());
   ReceiveOneLine();

   if (! buf_string_.empty())
      if (0 == buf_string_.compare(0,2,msg.str(),0,2))
      {
         intensity = atol(buf_string_.substr(2,3).c_str());
      }

}

void Controller::SetState(long state)
{
   state_ = state;
   stringstream msg;
   Illuminate();

   // Set timer for the Busy signal
   changedTime_ = GetCurrentMMTime();
}

void Controller::GetState(long &state)
{
   if (triggerMode_ == OFF) {
      Purge();
      Send("C?");
      long stateTmp = 0;

      for (unsigned int i=0;i<channelLetters_.size();i++)
      {
         ReceiveOneLine();

         if (! buf_string_.empty())
            if (buf_string_[5]=='N')
               stateTmp = 1;
      }
      state = stateTmp;
   }
   else
      state = state_;

}

int Controller::HandleErrors()
{
   int lastError = error_;
   error_ = 0;
   return lastError;
}

/////////////////////////////////////
//  Communications
/////////////////////////////////////

void Controller::Send(string cmd)
{
   int ret = SendSerialCommand(port_.c_str(), cmd.c_str(), carriage_return);
   if (ret!=DEVICE_OK)
      error_ = DEVICE_SERIAL_COMMAND_FAILED;
}

void Controller::ReceiveOneLine()
{
   buf_string_ = "";
   GetSerialAnswer(port_.c_str(), line_feed, buf_string_);

}

void Controller::Purge()
{
   int ret = PurgeComPort(port_.c_str());
   if (ret!=0)
      error_ = DEVICE_SERIAL_COMMAND_FAILED;
}

//********************
// Shutter API
//********************

int Controller::SetOpen(bool open)
{
   SetState((long) open);
   return HandleErrors();
}

int Controller::GetOpen(bool& open)
{
   long state;
   GetState(state);
   if (state==1)
      open = true;
   else if (state==0)
      open = false;
   else
      error_ = DEVICE_UNKNOWN_POSITION;

   return HandleErrors();
}

int Controller::Fire(double deltaT)
{
   deltaT=0; // Suppress warning
   error_ = DEVICE_UNSUPPORTED_COMMAND;
   return HandleErrors();
}
<|endoftext|>


<commit_before>/* /~\_.-~-._,--.._
  |                ~-.._
   |     .     .        \
   |                    /
   |     --....,       | \
   /                      \
  |                        |
  |                        \
  /                         |
  \_.-._  __...___.._______/
        ~~                   */

#include <stdexcept>

#include <tdpi/tdpi.h>
#include <GLFW/glfw3.h>

#include "tppi/window.hpp"

using namespace tppi;

struct glfw_window_deleter {
    void operator()(GLFWwindow* glfw_window) { glfwDestroyWindow(glfw_window); }
};

class window::window_impl {
public:
    window_impl(unsigned int width, unsigned int height, const std::string& title, bool fullscreen, bool resizable) {
        if (!glfwInit()) throw std::runtime_error("Failed to initialize GLFW.");

        glfwSetWindowUserPointer(glfw_window.get(), this);

        glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
        glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);

        glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
        glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

        glfwWindowHint(GLFW_RESIZABLE, (resizable ? GL_TRUE : GL_FALSE));

        glfw_window.reset(glfwCreateWindow(width, height, title.c_str(), (fullscreen ? glfwGetPrimaryMonitor() : nullptr), nullptr));
        if (!glfw_window.get()) throw std::runtime_error("Failed to create an OpenGL 3.3 context.");

        make_current();

        if (!tdpiLoadGL()) throw std::runtime_error("Failed to load OpenGL functions.");
    }

    ~window_impl() {
        glfwSetScrollCallback(glfw_window.get(), nullptr);
        glfwSetWindowFocusCallback(glfw_window.get(), nullptr);
    }

    void make_current() noexcept {
        glfwMakeContextCurrent(glfw_window.get());

        glfwSetScrollCallback(glfw_window.get(), set_scroll_offset);
        glfwSetWindowFocusCallback(glfw_window.get(), set_focused);
    }

    void resize(unsigned int width, unsigned int height) noexcept { glfwSetWindowSize(glfw_window.get(), width, height); }
    void title(const std::string& new_title) { glfwSetWindowTitle(glfw_window.get(), new_title.c_str()); }

    bool button_is_pressed(unsigned int key) const noexcept { return glfwGetKey(glfw_window.get(), key); }

    double scroll_offset() const noexcept { return scroll_offset_; }
    bool focused() const noexcept { return focused_; }

    cursor_pos cursor_position() const noexcept {
        cursor_pos cursor_pos_{0, 0};
        glfwGetCursorPos(glfw_window.get(), &cursor_pos_.x, &cursor_pos_.y);

        return cursor_pos_;
    }

    void cursor_position(cursor_pos cursor_pos_) noexcept { glfwSetCursorPos(glfw_window.get(), cursor_pos_.x, cursor_pos_.y); }

    std::string clipboard_string() { return glfwGetClipboardString(glfw_window.get()); }
    void clipboard_string(const std::string& new_clipboard_string) { glfwSetClipboardString(glfw_window.get(), new_clipboard_string.c_str()); }
private:
    std::unique_ptr<GLFWwindow, glfw_window_deleter> glfw_window;

    static void set_scroll_offset(GLFWwindow* window_ptr, double new_scroll_offset, double) noexcept { static_cast<window_impl*>(glfwGetWindowUserPointer(window_ptr))->scroll_offset_ = new_scroll_offset; }
    static void set_focused(GLFWwindow* window_ptr, int is_window_in_focus) { static_cast<window_impl*>(glfwGetWindowUserPointer(window_ptr))->focused_ = is_window_in_focus; }
    double scroll_offset_ = 0;
    bool focused_ = false;
};

window::window(unsigned int width, unsigned int height, const std::string& title, bool fullscreen, bool resizable) : window_impl_(std::make_unique<window_impl>(width, height, title, fullscreen, resizable)) {
    if (reference_count > 0)
        throw std::runtime_error("Multiple windows aren't yet supported.");

    ++reference_count;
    current_window.reset(this);
}

window::~window() {
    --reference_count;
    if (reference_count == 0) glfwTerminate();

    current_window.reset();
}

void window::make_current() noexcept {
    window_impl_->make_current();
    current_window.reset(this);
}

void window::resize(unsigned int width, unsigned int height) noexcept { window_impl_->resize(width, height); }
void window::title(const std::string& new_title) { window_impl_->title(new_title); }

bool window::button_is_pressed(button button_) const noexcept { return window_impl_->button_is_pressed(static_cast<unsigned int>(button_)); }

double window::scroll_offset() const noexcept { return window_impl_->scroll_offset(); }
bool window::focused() const noexcept { return window_impl_->focused(); }

cursor_pos window::cursor_position() const noexcept { return window_impl_->cursor_position(); }
void window::cursor_position(cursor_pos cursor_pos_) noexcept { window_impl_->cursor_position(cursor_pos_); }

std::string window::clipboard_string() { return window_impl_->clipboard_string(); }
void window::clipboard_string(const std::string& new_clipboard_string) { window_impl_->clipboard_string(new_clipboard_string); }

unsigned int window::reference_count = 0;
std::unique_ptr<window> window::current_window = nullptr;
<commit_msg>One-line control structures should be in one line.<commit_after>/* /~\_.-~-._,--.._
  |                ~-.._
   |     .     .        \
   |                    /
   |     --....,       | \
   /                      \
  |                        |
  |                        \
  /                         |
  \_.-._  __...___.._______/
        ~~                   */

#include <stdexcept>

#include <tdpi/tdpi.h>
#include <GLFW/glfw3.h>

#include "tppi/window.hpp"

using namespace tppi;

struct glfw_window_deleter {
    void operator()(GLFWwindow* glfw_window) { glfwDestroyWindow(glfw_window); }
};

class window::window_impl {
public:
    window_impl(unsigned int width, unsigned int height, const std::string& title, bool fullscreen, bool resizable) {
        if (!glfwInit()) throw std::runtime_error("Failed to initialize GLFW.");

        glfwSetWindowUserPointer(glfw_window.get(), this);

        glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
        glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);

        glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
        glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

        glfwWindowHint(GLFW_RESIZABLE, (resizable ? GL_TRUE : GL_FALSE));

        glfw_window.reset(glfwCreateWindow(width, height, title.c_str(), (fullscreen ? glfwGetPrimaryMonitor() : nullptr), nullptr));
        if (!glfw_window.get()) throw std::runtime_error("Failed to create an OpenGL 3.3 context.");

        make_current();

        if (!tdpiLoadGL()) throw std::runtime_error("Failed to load OpenGL functions.");
    }

    ~window_impl() {
        glfwSetScrollCallback(glfw_window.get(), nullptr);
        glfwSetWindowFocusCallback(glfw_window.get(), nullptr);
    }

    void make_current() noexcept {
        glfwMakeContextCurrent(glfw_window.get());

        glfwSetScrollCallback(glfw_window.get(), set_scroll_offset);
        glfwSetWindowFocusCallback(glfw_window.get(), set_focused);
    }

    void resize(unsigned int width, unsigned int height) noexcept { glfwSetWindowSize(glfw_window.get(), width, height); }
    void title(const std::string& new_title) { glfwSetWindowTitle(glfw_window.get(), new_title.c_str()); }

    bool button_is_pressed(unsigned int key) const noexcept { return glfwGetKey(glfw_window.get(), key); }

    double scroll_offset() const noexcept { return scroll_offset_; }
    bool focused() const noexcept { return focused_; }

    cursor_pos cursor_position() const noexcept {
        cursor_pos cursor_pos_{0, 0};
        glfwGetCursorPos(glfw_window.get(), &cursor_pos_.x, &cursor_pos_.y);

        return cursor_pos_;
    }

    void cursor_position(cursor_pos cursor_pos_) noexcept { glfwSetCursorPos(glfw_window.get(), cursor_pos_.x, cursor_pos_.y); }

    std::string clipboard_string() { return glfwGetClipboardString(glfw_window.get()); }
    void clipboard_string(const std::string& new_clipboard_string) { glfwSetClipboardString(glfw_window.get(), new_clipboard_string.c_str()); }
private:
    std::unique_ptr<GLFWwindow, glfw_window_deleter> glfw_window;

    static void set_scroll_offset(GLFWwindow* window_ptr, double new_scroll_offset, double) noexcept { static_cast<window_impl*>(glfwGetWindowUserPointer(window_ptr))->scroll_offset_ = new_scroll_offset; }
    static void set_focused(GLFWwindow* window_ptr, int is_window_in_focus) { static_cast<window_impl*>(glfwGetWindowUserPointer(window_ptr))->focused_ = is_window_in_focus; }
    double scroll_offset_ = 0;
    bool focused_ = false;
};

window::window(unsigned int width, unsigned int height, const std::string& title, bool fullscreen, bool resizable) : window_impl_(std::make_unique<window_impl>(width, height, title, fullscreen, resizable)) {
    if (reference_count > 0) throw std::runtime_error("Multiple windows aren't yet supported.");

    ++reference_count;
    current_window.reset(this);
}

window::~window() {
    --reference_count;
    if (reference_count == 0) glfwTerminate();

    current_window.reset();
}

void window::make_current() noexcept {
    window_impl_->make_current();
    current_window.reset(this);
}

void window::resize(unsigned int width, unsigned int height) noexcept { window_impl_->resize(width, height); }
void window::title(const std::string& new_title) { window_impl_->title(new_title); }

bool window::button_is_pressed(button button_) const noexcept { return window_impl_->button_is_pressed(static_cast<unsigned int>(button_)); }

double window::scroll_offset() const noexcept { return window_impl_->scroll_offset(); }
bool window::focused() const noexcept { return window_impl_->focused(); }

cursor_pos window::cursor_position() const noexcept { return window_impl_->cursor_position(); }
void window::cursor_position(cursor_pos cursor_pos_) noexcept { window_impl_->cursor_position(cursor_pos_); }

std::string window::clipboard_string() { return window_impl_->clipboard_string(); }
void window::clipboard_string(const std::string& new_clipboard_string) { window_impl_->clipboard_string(new_clipboard_string); }

unsigned int window::reference_count = 0;
std::unique_ptr<window> window::current_window = nullptr;
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Wim Meeussen */

#include <boost/algorithm/string.hpp>
#include <ros/ros.h>
#include <vector>
#include "urdf/model.h"

namespace urdf{

Model::Model()
{
  this->clear();
}

void Model::clear()
{
  name_.clear();
  this->links_.clear();
  this->joints_.clear();
  this->materials_.clear();
  this->root_link_.reset();
}

bool Model::initFile(const std::string& filename)
{
  TiXmlDocument xml_doc;
  xml_doc.LoadFile(filename);

  return initXml(&xml_doc);
}

bool Model::initString(const std::string& xml_string)
{
  TiXmlDocument xml_doc;
  xml_doc.Parse(xml_string.c_str());

  return initXml(&xml_doc);
}

bool Model::initXml(TiXmlDocument *xml_doc)
{
  if (!xml_doc)
  {
    ROS_ERROR("Could not parse the xml");
    return false;
  }

  TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
  if (!robot_xml)
  {
    ROS_ERROR("Could not find the 'robot' element in the xml file");
    return false;
  }
  return initXml(robot_xml);
}

bool Model::initXml(TiXmlElement *robot_xml)
{
  this->clear();

  ROS_DEBUG("Parsing robot xml");
  if (!robot_xml) return false;

  // Get robot name
  const char *name = robot_xml->Attribute("name");
  if (!name)
  {
    ROS_ERROR("No name given for the robot.");
    return false;
  }
  this->name_ = std::string(name);

  // Get all Material elements
  for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
  {
    boost::shared_ptr<Material> material;
    material.reset(new Material);

    if (material->initXml(material_xml))
    {
      if (this->getMaterial(material->name))
      {
        ROS_ERROR("material '%s' is not unique.", material->name.c_str());
        material.reset();
        return false;
      }
      else
      {
        this->materials_.insert(make_pair(material->name,material));
        ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("material xml is not initialized correctly");
      material.reset();
      return false;
    }
  }

  // Get all Link elements
  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
  {
    boost::shared_ptr<Link> link;
    link.reset(new Link);

    if (link->initXml(link_xml))
    {
      if (this->getLink(link->name))
      {
        ROS_ERROR("link '%s' is not unique.", link->name.c_str());
        link.reset();
        return false;
      }
      else
      {
        // set link visual material
        ROS_DEBUG("setting link '%s' material", link->name.c_str());
        if (link->visual)
        {
          if (!link->visual->material_name.empty())
          {
            if (this->getMaterial(link->visual->material_name))
            {
              ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
              link->visual->material = this->getMaterial( link->visual->material_name.c_str() );
            }
            else
            {
              if (link->visual->material)
              {
                ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
                this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
              }
              else
              {
                ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
                link.reset();
                return false;
              }
            }
          }
        }

        this->links_.insert(make_pair(link->name,link));
        ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("link xml is not initialized correctly");
      link.reset();
      return false;
    }
  }
  // Get all Joint elements
  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
  {
    boost::shared_ptr<Joint> joint;
    joint.reset(new Joint);

    if (joint->initXml(joint_xml))
    {
      if (this->getJoint(joint->name))
      {
        ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
        joint.reset();
        return false;
      }
      else
      {
        this->joints_.insert(make_pair(joint->name,joint));
        ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("joint xml is not initialized correctly");
      joint.reset();
      return false;
    }
  }

  // every link has children links and joints, but no parents, so we create a
  // local convenience data structure for keeping child->parent relations
  std::map<std::string, std::string> parent_link_tree;
  parent_link_tree.clear();

  // building tree: name mapping
  if (!this->initTree(parent_link_tree))
  {
    ROS_ERROR("failed to build tree");
    return false;
  }

  // find the root link
  if (!this->initRoot(parent_link_tree))
  {
    ROS_ERROR("failed to find root link");
    return false;
  }

  return true;
}

bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
{
  // loop through all joints, for every link, assign children links and children joints
  for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
  {
    std::string parent_link_name = joint->second->parent_link_name;
    std::string child_link_name = joint->second->child_link_name;

    ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());

    /// add an empty "world" link
    if (parent_link_name == "world")
    {
      if (this->getLink(parent_link_name))
      {
        ROS_DEBUG("    parent link '%s' already exists.", parent_link_name.c_str());
      }
      else
      {
        ROS_DEBUG("    parent link '%s' is a special case, adding fake link.", parent_link_name.c_str());
        boost::shared_ptr<Link> link;
        link.reset(new Link);
        link->name = "world";
        this->links_.insert(make_pair(link->name,link));
        ROS_DEBUG("        successfully added new link '%s'", link->name.c_str());
      }
    }

    if (parent_link_name.empty())
    {
      ROS_DEBUG("    Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
    }
    else if (child_link_name.empty())
    {
      ROS_DEBUG("    Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
    }
    else
    {
      // find parent link
      boost::shared_ptr<Link> parent_link;
      this->getLink(parent_link_name, parent_link);

      if (!parent_link)
      {
        ROS_ERROR("    parent link '%s' of joint '%s' not found", parent_link_name.c_str(), joint->first.c_str() );
        return false;
      }
      else
      {
        // find child link
        boost::shared_ptr<Link> child_link;
        this->getLink(child_link_name, child_link);

        if (!child_link)
        {
          ROS_ERROR("    child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str());
          return false;
        }
        else
        {
          //set parent link for child link
          child_link->setParent(parent_link);

          //set parent joint for child link
          child_link->setParentJoint(joint->second);

          //set child joint for parent link
          parent_link->addChildJoint(joint->second);

          //set child link for parent link
          parent_link->addChild(child_link);

          // fill in child/parent string map
          parent_link_tree[child_link->name] = parent_link_name;

          ROS_DEBUG("    now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
        }
      }
    }
  }

  return true;
}

bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
{

  this->root_link_.reset();

  for (std::map<std::string, std::string>::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++)
  {
    if (parent_link_tree.find(p->second) == parent_link_tree.end())
    {
      if (this->root_link_)
      {
        ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str());
        if (this->root_link_->name != p->second)
        {
          ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str());
          return false;
        }
      }
      else
         getLink(p->second,this->root_link_);
    }
  }
  if (!this->root_link_)
  {
    ROS_ERROR("No root link found. The robot xml is empty or not a tree.");
    return false;
  }
  ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());

  return true;
}

boost::shared_ptr<Material> Model::getMaterial(const std::string& name) const
{
  boost::shared_ptr<Material> ptr;
  if (this->materials_.find(name) == this->materials_.end())
    ptr.reset();
  else
    ptr = this->materials_.find(name)->second;
  return ptr;
}

boost::shared_ptr<const Link> Model::getLink(const std::string& name) const
{
  boost::shared_ptr<const Link> ptr;
  if (this->links_.find(name) == this->links_.end())
    ptr.reset();
  else
    ptr = this->links_.find(name)->second;
  return ptr;
}

void Model::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
  for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
  {
    links.push_back(link->second);
  }
}

void Model::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
  boost::shared_ptr<Link> ptr;
  if (this->links_.find(name) == this->links_.end())
    ptr.reset();
  else
    ptr = this->links_.find(name)->second;
  link = ptr;
}

boost::shared_ptr<const Joint> Model::getJoint(const std::string& name) const
{
  boost::shared_ptr<const Joint> ptr;
  if (this->joints_.find(name) == this->joints_.end())
    ptr.reset();
  else
    ptr = this->joints_.find(name)->second;
  return ptr;
}

}

<commit_msg>Give better error message when no links were found<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Wim Meeussen */

#include <boost/algorithm/string.hpp>
#include <ros/ros.h>
#include <vector>
#include "urdf/model.h"

namespace urdf{

Model::Model()
{
  this->clear();
}

void Model::clear()
{
  name_.clear();
  this->links_.clear();
  this->joints_.clear();
  this->materials_.clear();
  this->root_link_.reset();
}

bool Model::initFile(const std::string& filename)
{
  TiXmlDocument xml_doc;
  xml_doc.LoadFile(filename);

  return initXml(&xml_doc);
}

bool Model::initString(const std::string& xml_string)
{
  TiXmlDocument xml_doc;
  xml_doc.Parse(xml_string.c_str());

  return initXml(&xml_doc);
}

bool Model::initXml(TiXmlDocument *xml_doc)
{
  if (!xml_doc)
  {
    ROS_ERROR("Could not parse the xml");
    return false;
  }

  TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
  if (!robot_xml)
  {
    ROS_ERROR("Could not find the 'robot' element in the xml file");
    return false;
  }
  return initXml(robot_xml);
}

bool Model::initXml(TiXmlElement *robot_xml)
{
  this->clear();

  ROS_DEBUG("Parsing robot xml");
  if (!robot_xml) return false;

  // Get robot name
  const char *name = robot_xml->Attribute("name");
  if (!name)
  {
    ROS_ERROR("No name given for the robot.");
    return false;
  }
  this->name_ = std::string(name);

  // Get all Material elements
  for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
  {
    boost::shared_ptr<Material> material;
    material.reset(new Material);

    if (material->initXml(material_xml))
    {
      if (this->getMaterial(material->name))
      {
        ROS_ERROR("material '%s' is not unique.", material->name.c_str());
        material.reset();
        return false;
      }
      else
      {
        this->materials_.insert(make_pair(material->name,material));
        ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("material xml is not initialized correctly");
      material.reset();
      return false;
    }
  }

  // Get all Link elements
  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
  {
    boost::shared_ptr<Link> link;
    link.reset(new Link);

    if (link->initXml(link_xml))
    {
      if (this->getLink(link->name))
      {
        ROS_ERROR("link '%s' is not unique.", link->name.c_str());
        link.reset();
        return false;
      }
      else
      {
        // set link visual material
        ROS_DEBUG("setting link '%s' material", link->name.c_str());
        if (link->visual)
        {
          if (!link->visual->material_name.empty())
          {
            if (this->getMaterial(link->visual->material_name))
            {
              ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
              link->visual->material = this->getMaterial( link->visual->material_name.c_str() );
            }
            else
            {
              if (link->visual->material)
              {
                ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
                this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
              }
              else
              {
                ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
                link.reset();
                return false;
              }
            }
          }
        }

        this->links_.insert(make_pair(link->name,link));
        ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("link xml is not initialized correctly");
      link.reset();
      return false;
    }
  }
  // Get all Joint elements
  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
  {
    boost::shared_ptr<Joint> joint;
    joint.reset(new Joint);

    if (joint->initXml(joint_xml))
    {
      if (this->getJoint(joint->name))
      {
        ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
        joint.reset();
        return false;
      }
      else
      {
        this->joints_.insert(make_pair(joint->name,joint));
        ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
      }
    }
    else
    {
      ROS_ERROR("joint xml is not initialized correctly");
      joint.reset();
      return false;
    }
  }

  // every link has children links and joints, but no parents, so we create a
  // local convenience data structure for keeping child->parent relations
  std::map<std::string, std::string> parent_link_tree;
  parent_link_tree.clear();

  // building tree: name mapping
  if (!this->initTree(parent_link_tree))
  {
    ROS_ERROR("failed to build tree");
    return false;
  }

  // make sure tree is not empty
  if (parent_link_tree.empty()){
    ROS_ERROR("The robot xml does not contain any valid links. Are you parsing an empty file, or an un-processed xacro file?");
    return false;
  }

  // find the root link
  if (!this->initRoot(parent_link_tree))
  {
    ROS_ERROR("failed to find root link");
    return false;
  }

  return true;
}

bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
{
  // loop through all joints, for every link, assign children links and children joints
  for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
  {
    std::string parent_link_name = joint->second->parent_link_name;
    std::string child_link_name = joint->second->child_link_name;

    ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());

    /// add an empty "world" link
    if (parent_link_name == "world")
    {
      if (this->getLink(parent_link_name))
      {
        ROS_DEBUG("    parent link '%s' already exists.", parent_link_name.c_str());
      }
      else
      {
        ROS_DEBUG("    parent link '%s' is a special case, adding fake link.", parent_link_name.c_str());
        boost::shared_ptr<Link> link;
        link.reset(new Link);
        link->name = "world";
        this->links_.insert(make_pair(link->name,link));
        ROS_DEBUG("        successfully added new link '%s'", link->name.c_str());
      }
    }

    if (parent_link_name.empty())
    {
      ROS_DEBUG("    Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
    }
    else if (child_link_name.empty())
    {
      ROS_DEBUG("    Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
    }
    else
    {
      // find parent link
      boost::shared_ptr<Link> parent_link;
      this->getLink(parent_link_name, parent_link);

      if (!parent_link)
      {
        ROS_ERROR("    parent link '%s' of joint '%s' not found", parent_link_name.c_str(), joint->first.c_str() );
        return false;
      }
      else
      {
        // find child link
        boost::shared_ptr<Link> child_link;
        this->getLink(child_link_name, child_link);

        if (!child_link)
        {
          ROS_ERROR("    child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str());
          return false;
        }
        else
        {
          //set parent link for child link
          child_link->setParent(parent_link);

          //set parent joint for child link
          child_link->setParentJoint(joint->second);

          //set child joint for parent link
          parent_link->addChildJoint(joint->second);

          //set child link for parent link
          parent_link->addChild(child_link);

          // fill in child/parent string map
          parent_link_tree[child_link->name] = parent_link_name;

          ROS_DEBUG("    now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
        }
      }
    }
  }

  return true;
}

bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
{

  this->root_link_.reset();

  for (std::map<std::string, std::string>::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++)
  {
    if (parent_link_tree.find(p->second) == parent_link_tree.end())
    {
      if (this->root_link_)
      {
        ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str());
        if (this->root_link_->name != p->second)
        {
          ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str());
          return false;
        }
      }
      else
         getLink(p->second,this->root_link_);
    }
  }
  if (!this->root_link_)
  {
    ROS_ERROR("No root link found. The robot xml is not a valid tree.");
    return false;
  }
  ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());

  return true;
}

boost::shared_ptr<Material> Model::getMaterial(const std::string& name) const
{
  boost::shared_ptr<Material> ptr;
  if (this->materials_.find(name) == this->materials_.end())
    ptr.reset();
  else
    ptr = this->materials_.find(name)->second;
  return ptr;
}

boost::shared_ptr<const Link> Model::getLink(const std::string& name) const
{
  boost::shared_ptr<const Link> ptr;
  if (this->links_.find(name) == this->links_.end())
    ptr.reset();
  else
    ptr = this->links_.find(name)->second;
  return ptr;
}

void Model::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
  for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
  {
    links.push_back(link->second);
  }
}

void Model::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
  boost::shared_ptr<Link> ptr;
  if (this->links_.find(name) == this->links_.end())
    ptr.reset();
  else
    ptr = this->links_.find(name)->second;
  link = ptr;
}

boost::shared_ptr<const Joint> Model::getJoint(const std::string& name) const
{
  boost::shared_ptr<const Joint> ptr;
  if (this->joints_.find(name) == this->joints_.end())
    ptr.reset();
  else
    ptr = this->joints_.find(name)->second;
  return ptr;
}

}

<|endoftext|>


<commit_before>/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#include <af/defines.h>
#include <program.hpp>
#include <dispatch.hpp>
#include <err_opencl.hpp>
#include <debug_opencl.hpp>
#include <kernel_headers/nearest_neighbour.hpp>
#include <memory.hpp>
#include <math.hpp>

using cl::LocalSpaceArg;

namespace opencl
{

namespace kernel
{

static const unsigned THREADS = 256;

template<typename T, typename To, af_match_type dist_type, bool use_lmem, unsigned unroll_len>
void nearest_neighbour(Param idx,
                       Param dist,
                       Param query,
                       Param train,
                       const dim_t dist_dim,
                       const unsigned n_dist,
                       const size_t lmem_sz)
{
    try {
        static std::once_flag compileFlags[DeviceManager::MAX_DEVICES];
        static Program        nearest_neighbourProgs[DeviceManager::MAX_DEVICES];
        static Kernel             huKernel[DeviceManager::MAX_DEVICES];
        static Kernel             hmKernel[DeviceManager::MAX_DEVICES];
        static Kernel             smKernel[DeviceManager::MAX_DEVICES];

        int device = getActiveDeviceId();

        const unsigned feat_len = query.info.dims[dist_dim];
        const To max_dist = limit_max<To>();

        if (feat_len > THREADS) {
            OPENCL_NOT_SUPPORTED();
        }

        std::call_once( compileFlags[device], [device] () {

                std::ostringstream options;
                options << " -D T=" << dtype_traits<T>::getName()
                        << " -D To=" << dtype_traits<To>::getName()
                        << " -D THREADS=" << THREADS
                        << " -D FEAT_LEN=" << unroll_len;

                switch(dist_type) {
                    case AF_SAD: options <<" -D DISTOP=_sad_"; break;
                    case AF_SSD: options <<" -D DISTOP=_ssd_"; break;
                    case AF_SHD: options <<" -D DISTOP=_shd_ -D __SHD__";
                                 break;
                    default: break;
                }

                if (use_lmem)
                    options << " -D USE_LOCAL_MEM";

                buildProgram(nearest_neighbourProgs[device],
                             nearest_neighbour_cl,
                             nearest_neighbour_cl_len,
                             options.str());

                huKernel[device] = Kernel(nearest_neighbourProgs[device], "nearest_neighbour_unroll");
                hmKernel[device] = Kernel(nearest_neighbourProgs[device], "nearest_neighbour");
                smKernel[device] = Kernel(nearest_neighbourProgs[device], "select_matches");
            });

        const dim_t sample_dim = (dist_dim == 0) ? 1 : 0;

        const unsigned nquery = query.info.dims[sample_dim];
        const unsigned ntrain = train.info.dims[sample_dim];

        unsigned nblk = divup(ntrain, THREADS);
        const NDRange local(THREADS, 1);
        const NDRange global(nblk * THREADS, 1);

        cl::Buffer *d_blk_idx  = bufferAlloc(nblk * nquery * sizeof(unsigned));
        cl::Buffer *d_blk_dist = bufferAlloc(nblk * nquery * sizeof(To));

        // For each query vector, find training vector with smallest Hamming
        // distance per CUDA block
        if (unroll_len > 0) {
            auto huOp = make_kernel<Buffer, Buffer,
                                    Buffer, KParam,
                                    Buffer, KParam,
                                    const To,
                                    LocalSpaceArg> (huKernel[device]);

            huOp(EnqueueArgs(getQueue(), global, local),
                 *d_blk_idx, *d_blk_dist,
                 *query.data, query.info, *train.data, train.info,
                 max_dist, cl::Local(lmem_sz));
        }
        else {
            auto hmOp = make_kernel<Buffer, Buffer,
                                    Buffer, KParam,
                                    Buffer, KParam,
                                    const To, const unsigned,
                                    LocalSpaceArg> (hmKernel[device]);

            hmOp(EnqueueArgs(getQueue(), global, local),
                 *d_blk_idx, *d_blk_dist,
                 *query.data, query.info, *train.data, train.info,
                 max_dist, feat_len, cl::Local(lmem_sz));
        }
        CL_DEBUG_FINISH(getQueue());

        const NDRange local_sm(32, 8);
        const NDRange global_sm(divup(nquery, 32) * 32, 8);

        // Reduce all smallest Hamming distances from each block and store final
        // best match
        auto smOp = make_kernel<Buffer, Buffer, Buffer, Buffer,
                                const unsigned, const unsigned,
                                const To> (smKernel[device]);

        smOp(EnqueueArgs(getQueue(), global_sm, local_sm),
             *idx.data, *dist.data,
             *d_blk_idx, *d_blk_dist,
             nquery, nblk, max_dist);
        CL_DEBUG_FINISH(getQueue());

        bufferFree(d_blk_idx);
        bufferFree(d_blk_dist);
    } catch (cl::Error err) {
        CL_TO_AF_ERROR(err);
        throw;
    }
}

} // namespace kernel

} // namespace opencl
<commit_msg>Fix double compilation<commit_after>/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#include <af/defines.h>
#include <program.hpp>
#include <dispatch.hpp>
#include <err_opencl.hpp>
#include <debug_opencl.hpp>
#include <kernel_headers/nearest_neighbour.hpp>
#include <memory.hpp>
#include <math.hpp>

using cl::LocalSpaceArg;

namespace opencl
{

namespace kernel
{

static const unsigned THREADS = 256;

template<typename T, typename To, af_match_type dist_type, bool use_lmem, unsigned unroll_len>
void nearest_neighbour(Param idx,
                       Param dist,
                       Param query,
                       Param train,
                       const dim_t dist_dim,
                       const unsigned n_dist,
                       const size_t lmem_sz)
{
    try {
        static std::once_flag compileFlags[DeviceManager::MAX_DEVICES];
        static Program        nearest_neighbourProgs[DeviceManager::MAX_DEVICES];
        static Kernel             huKernel[DeviceManager::MAX_DEVICES];
        static Kernel             hmKernel[DeviceManager::MAX_DEVICES];
        static Kernel             smKernel[DeviceManager::MAX_DEVICES];

        int device = getActiveDeviceId();

        const unsigned feat_len = query.info.dims[dist_dim];
        const To max_dist = limit_max<To>();

        if (feat_len > THREADS) {
            OPENCL_NOT_SUPPORTED();
        }

        std::call_once( compileFlags[device], [device] () {

                std::ostringstream options;
                options << " -D T=" << dtype_traits<T>::getName()
                        << " -D To=" << dtype_traits<To>::getName()
                        << " -D THREADS=" << THREADS
                        << " -D FEAT_LEN=" << unroll_len;

                switch(dist_type) {
                    case AF_SAD: options <<" -D DISTOP=_sad_"; break;
                    case AF_SSD: options <<" -D DISTOP=_ssd_"; break;
                    case AF_SHD: options <<" -D DISTOP=_shd_ -D __SHD__";
                                 break;
                    default: break;
                }

                if (std::is_same<T, double>::value ||
                    std::is_same<T, cdouble>::value) {
                    options << " -D USE_DOUBLE";
                }

                if (use_lmem)
                    options << " -D USE_LOCAL_MEM";

                buildProgram(nearest_neighbourProgs[device],
                             nearest_neighbour_cl,
                             nearest_neighbour_cl_len,
                             options.str());

                huKernel[device] = Kernel(nearest_neighbourProgs[device], "nearest_neighbour_unroll");
                hmKernel[device] = Kernel(nearest_neighbourProgs[device], "nearest_neighbour");
                smKernel[device] = Kernel(nearest_neighbourProgs[device], "select_matches");
            });

        const dim_t sample_dim = (dist_dim == 0) ? 1 : 0;

        const unsigned nquery = query.info.dims[sample_dim];
        const unsigned ntrain = train.info.dims[sample_dim];

        unsigned nblk = divup(ntrain, THREADS);
        const NDRange local(THREADS, 1);
        const NDRange global(nblk * THREADS, 1);

        cl::Buffer *d_blk_idx  = bufferAlloc(nblk * nquery * sizeof(unsigned));
        cl::Buffer *d_blk_dist = bufferAlloc(nblk * nquery * sizeof(To));

        // For each query vector, find training vector with smallest Hamming
        // distance per CUDA block
        if (unroll_len > 0) {
            auto huOp = make_kernel<Buffer, Buffer,
                                    Buffer, KParam,
                                    Buffer, KParam,
                                    const To,
                                    LocalSpaceArg> (huKernel[device]);

            huOp(EnqueueArgs(getQueue(), global, local),
                 *d_blk_idx, *d_blk_dist,
                 *query.data, query.info, *train.data, train.info,
                 max_dist, cl::Local(lmem_sz));
        }
        else {
            auto hmOp = make_kernel<Buffer, Buffer,
                                    Buffer, KParam,
                                    Buffer, KParam,
                                    const To, const unsigned,
                                    LocalSpaceArg> (hmKernel[device]);

            hmOp(EnqueueArgs(getQueue(), global, local),
                 *d_blk_idx, *d_blk_dist,
                 *query.data, query.info, *train.data, train.info,
                 max_dist, feat_len, cl::Local(lmem_sz));
        }
        CL_DEBUG_FINISH(getQueue());

        const NDRange local_sm(32, 8);
        const NDRange global_sm(divup(nquery, 32) * 32, 8);

        // Reduce all smallest Hamming distances from each block and store final
        // best match
        auto smOp = make_kernel<Buffer, Buffer, Buffer, Buffer,
                                const unsigned, const unsigned,
                                const To> (smKernel[device]);

        smOp(EnqueueArgs(getQueue(), global_sm, local_sm),
             *idx.data, *dist.data,
             *d_blk_idx, *d_blk_dist,
             nquery, nblk, max_dist);
        CL_DEBUG_FINISH(getQueue());

        bufferFree(d_blk_idx);
        bufferFree(d_blk_dist);
    } catch (cl::Error err) {
        CL_TO_AF_ERROR(err);
        throw;
    }
}

} // namespace kernel

} // namespace opencl
<|endoftext|>


<commit_before>#include "ofxFps.h"

#ifdef TARGET_WIN32
#ifndef _MSC_VER
#include <unistd.h> // this if for MINGW / _getcwd
#include <sys/param.h> // for MAXPATHLEN
#endif
#endif

#if defined(TARGET_OF_IOS) || defined(TARGET_OSX ) || defined(TARGET_LINUX) || defined(TARGET_EMSCRIPTEN)
#include <sys/time.h>
#endif

#ifdef TARGET_OSX
#ifndef TARGET_OF_IOS
#include <mach-o/dyld.h>
#include <sys/param.h> // for MAXPATHLEN
#endif
#include <mach/clock.h>
#include <mach/mach.h>
#endif

#ifdef TARGET_WIN32
#include <mmsystem.h>
#ifdef _MSC_VER
#include <direct.h>
#endif

#endif

struct less_first : std::binary_function<Tick,Tick,bool>
{
    inline bool operator()( const Tick& lhs, const Tick& rhs )
    {
        return lhs.second < rhs.second;
    }
};

ofxFps::ofxFps() {
	this->timeBegin = 0;
	this->timeEnd = 0;
	this->timeFrame = 0;
	this->timeUpdate = 0;
	this->minFramerate = 10.f;
	this->maxLoad = 0.9f;
}

unsigned long long ofxFps::getTimeMicros() {
#if (defined(TARGET_LINUX) && !defined(TARGET_RASPBERRY_PI)) || defined(TARGET_EMSCRIPTEN)
    struct timespec now;
    clock_gettime(CLOCK_MONOTONIC, &now);
    return now.tv_sec * 1000000 + now.tv_nsec / 1000;
#elif defined(TARGET_OSX)
    clock_serv_t cs;
    mach_timespec_t now;
    host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cs);
    clock_get_time(cs, &now);
    mach_port_deallocate(mach_task_self(), cs);
    return now.tv_sec * 1000000 + now.tv_nsec / 1000;
#elif defined( TARGET_WIN32 )
    LARGE_INTEGER freq;
    LARGE_INTEGER counter;
    QueryPerformanceFrequency(&freq);
    QueryPerformanceCounter(&counter);
    return (counter.QuadPart / freq.QuadPart)*1000000 + (counter.QuadPart % freq.QuadPart)*1000000/freq.QuadPart;
#else
    struct timeval now;
    gettimeofday( &now, NULL );
    return now.tv_sec * 1000000 + now.tv_usec;
#endif
}

void ofxFps::begin() {
	register auto now = getTimeMicros();
	timeFrame = now - timeBegin;
	timeBegin = now;
    for (auto it=ticks.begin(); it!=ticks.begin(); ++it) {
        it->second = timeBegin;
    }
}

void ofxFps::tick(string name) {
    ticks[name] = getTimeMicros();
}

void ofxFps::end() {
	register unsigned long long now = getTimeMicros();
	timeFrame = now - timeEnd;
	timeUpdate = now - timeBegin;
	timeEnd = now;

    ticks_sorted.clear();
    ticks_sorted.assign(ticks.begin(), ticks.end());
    std::sort(ticks_sorted.begin(), ticks_sorted.end(), less_first());
}

float ofxFps::getFps() {
	return timeFrame != 0 ? 1000000. / timeFrame : 0.f;
}

float ofxFps::getLoad() {
	return ((float)timeUpdate / (float)timeFrame);
}

double ofxFps::getFrameTime() {
	return timeFrame / 1000000.;
}

float ofxFps::getFrameTimef() {
	return timeFrame / 1000000.;
}

unsigned int ofxFps::getFrameTimeMicros() {
	return timeFrame;
}

unsigned int ofxFps::getFrameTimeMillis() {
	return timeFrame / 1000;
}

string ofxFps::toString(int fpsPrecision, int loadPrecision, bool useTicks) {
    stringstream ss;
    ss << ofToString(getFps(), 1);
    ss << " fps (";

    if (useTicks && ticks.size() > 0) {
        vector<Tick>::iterator it=ticks_sorted.begin();
        unsigned long long previous = timeBegin;
        for (; it!=ticks_sorted.end(); ) {
            ss << it->first;
            ss << ": ";
            if (it->second > previous && timeFrame != 0) {
				float load = 100. * (float)(it->second - previous) / (float)timeFrame;
                ss << ofToString(load, loadPrecision);
			}
            else
                ss << "0";
            ss << " %";
            previous = it->second;
            if (++it != ticks_sorted.end())
                ss << ", ";
        }
        ss << ")";
    }
    else {
        ss << ofToString(getLoad()*100.f, 0);
        ss << " %)";
    }
    return ss.str();
}

void ofxFps::draw(int x, int y) {

    if (getFps() < minFramerate || getLoad() > maxLoad)
        ofSetColor(255, 0, 0);
    else
        ofSetColor(255);
    ofDrawBitmapString(toString(), x, y);
	ofSetColor(255);
}

void ofxFps::draw(int x, int y, string label, bool drawTicks) {

    if (getFps() <minFramerate || getLoad() > maxLoad)
        ofSetColor(255, 0, 0);
    else
        ofSetColor(255);
    ofDrawBitmapString(label + ": " + toString(1, 0, drawTicks), x, y);
	ofSetColor(255);
}

ofxFpsHistory::ofxFpsHistory(int size, bool autoMax) {
    setSize(size);
    this->autoMax = autoMax;
}

void ofxFpsHistory::setSize(int size) {
    this->size = size;
}

void ofxFpsHistory::setMax(float max) {
    this->max = max;
}

void ofxFpsHistory::setAutoMax(bool autoMax) {
    this->autoMax = autoMax;
}

void ofxFpsHistory::add(float f) {
    history.push_front(f);
    while (history.size() > size)
        history.pop_back();
}

void ofxFpsHistory::draw(float x, float y, float height) {

    if (autoMax) {
        max = 0.f;
        list<float>::iterator it=history.begin();
        for (; it!=history.end(); ++it) {
            if (*it > max)
                max = *it;
        }
    }
    list<float>::iterator it=history.begin();
    for (; it!=history.end(); ++it) {
        ofLine(x, y+height, x, y+height-(*it/max)*height);
        x++;
    }
}
<commit_msg>removed white set color<commit_after>#include "ofxFps.h"

#ifdef TARGET_WIN32
#ifndef _MSC_VER
#include <unistd.h> // this if for MINGW / _getcwd
#include <sys/param.h> // for MAXPATHLEN
#endif
#endif

#if defined(TARGET_OF_IOS) || defined(TARGET_OSX ) || defined(TARGET_LINUX) || defined(TARGET_EMSCRIPTEN)
#include <sys/time.h>
#endif

#ifdef TARGET_OSX
#ifndef TARGET_OF_IOS
#include <mach-o/dyld.h>
#include <sys/param.h> // for MAXPATHLEN
#endif
#include <mach/clock.h>
#include <mach/mach.h>
#endif

#ifdef TARGET_WIN32
#include <mmsystem.h>
#ifdef _MSC_VER
#include <direct.h>
#endif

#endif

struct less_first : std::binary_function<Tick,Tick,bool>
{
    inline bool operator()( const Tick& lhs, const Tick& rhs )
    {
        return lhs.second < rhs.second;
    }
};

ofxFps::ofxFps() {
	this->timeBegin = 0;
	this->timeEnd = 0;
	this->timeFrame = 0;
	this->timeUpdate = 0;
	this->minFramerate = 10.f;
	this->maxLoad = 0.9f;
}

unsigned long long ofxFps::getTimeMicros() {
#if (defined(TARGET_LINUX) && !defined(TARGET_RASPBERRY_PI)) || defined(TARGET_EMSCRIPTEN)
    struct timespec now;
    clock_gettime(CLOCK_MONOTONIC, &now);
    return now.tv_sec * 1000000 + now.tv_nsec / 1000;
#elif defined(TARGET_OSX)
    clock_serv_t cs;
    mach_timespec_t now;
    host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cs);
    clock_get_time(cs, &now);
    mach_port_deallocate(mach_task_self(), cs);
    return now.tv_sec * 1000000 + now.tv_nsec / 1000;
#elif defined( TARGET_WIN32 )
    LARGE_INTEGER freq;
    LARGE_INTEGER counter;
    QueryPerformanceFrequency(&freq);
    QueryPerformanceCounter(&counter);
    return (counter.QuadPart / freq.QuadPart)*1000000 + (counter.QuadPart % freq.QuadPart)*1000000/freq.QuadPart;
#else
    struct timeval now;
    gettimeofday( &now, NULL );
    return now.tv_sec * 1000000 + now.tv_usec;
#endif
}

void ofxFps::begin() {
	register auto now = getTimeMicros();
	timeFrame = now - timeBegin;
	timeBegin = now;
    for (auto it=ticks.begin(); it!=ticks.begin(); ++it) {
        it->second = timeBegin;
    }
}

void ofxFps::tick(string name) {
    ticks[name] = getTimeMicros();
}

void ofxFps::end() {
	register unsigned long long now = getTimeMicros();
	timeFrame = now - timeEnd;
	timeUpdate = now - timeBegin;
	timeEnd = now;

    ticks_sorted.clear();
    ticks_sorted.assign(ticks.begin(), ticks.end());
    std::sort(ticks_sorted.begin(), ticks_sorted.end(), less_first());
}

float ofxFps::getFps() {
	return timeFrame != 0 ? 1000000. / timeFrame : 0.f;
}

float ofxFps::getLoad() {
	return ((float)timeUpdate / (float)timeFrame);
}

double ofxFps::getFrameTime() {
	return timeFrame / 1000000.;
}

float ofxFps::getFrameTimef() {
	return timeFrame / 1000000.;
}

unsigned int ofxFps::getFrameTimeMicros() {
	return timeFrame;
}

unsigned int ofxFps::getFrameTimeMillis() {
	return timeFrame / 1000;
}

string ofxFps::toString(int fpsPrecision, int loadPrecision, bool useTicks) {
    stringstream ss;
    ss << ofToString(getFps(), 1);
    ss << " fps (";

    if (useTicks && ticks.size() > 0) {
        vector<Tick>::iterator it=ticks_sorted.begin();
        unsigned long long previous = timeBegin;
        for (; it!=ticks_sorted.end(); ) {
            ss << it->first;
            ss << ": ";
            if (it->second > previous && timeFrame != 0) {
				float load = 100. * (float)(it->second - previous) / (float)timeFrame;
                ss << ofToString(load, loadPrecision);
			}
            else
                ss << "0";
            ss << " %";
            previous = it->second;
            if (++it != ticks_sorted.end())
                ss << ", ";
        }
        ss << ")";
    }
    else {
        ss << ofToString(getLoad()*100.f, 0);
        ss << " %)";
    }
    return ss.str();
}

void ofxFps::draw(int x, int y) {

    if (getFps() < minFramerate || getLoad() > maxLoad)
        ofSetColor(255, 0, 0);

    ofDrawBitmapString(toString(), x, y);
	ofSetColor(255);
}

void ofxFps::draw(int x, int y, string label, bool drawTicks) {

    if (getFps() <minFramerate || getLoad() > maxLoad)
        ofSetColor(255, 0, 0);

    ofDrawBitmapString(label + ": " + toString(1, 0, drawTicks), x, y);
	ofSetColor(255);
}

ofxFpsHistory::ofxFpsHistory(int size, bool autoMax) {
    setSize(size);
    this->autoMax = autoMax;
}

void ofxFpsHistory::setSize(int size) {
    this->size = size;
}

void ofxFpsHistory::setMax(float max) {
    this->max = max;
}

void ofxFpsHistory::setAutoMax(bool autoMax) {
    this->autoMax = autoMax;
}

void ofxFpsHistory::add(float f) {
    history.push_front(f);
    while (history.size() > size)
        history.pop_back();
}

void ofxFpsHistory::draw(float x, float y, float height) {

    if (autoMax) {
        max = 0.f;
        list<float>::iterator it=history.begin();
        for (; it!=history.end(); ++it) {
            if (*it > max)
                max = *it;
        }
    }
    list<float>::iterator it=history.begin();
    for (; it!=history.end(); ++it) {
        ofLine(x, y+height, x, y+height-(*it/max)*height);
        x++;
    }
}
<|endoftext|>


<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _SV_SALDATA_HXX
#define _SV_SALDATA_HXX

// -=-= includes -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
#include <signal.h>
#include <salstd.hxx>
#include <vcl/salframe.hxx>
#include <salinst.h>
#include <vcl/saldatabasic.hxx>
#include <osl/module.h>
#include <vcl/dllapi.h>

// -=-= forwards -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
class SalXLib;
class SalDisplay;
class SalPrinter;

// -=-= typedefs -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

DECLARE_LIST( SalDisplays, SalDisplay* )

#if defined LINUX || defined NETBSD || defined AIX || \
    defined FREEBSD || defined OPENBSD || defined DRAGONFLY
#include <pthread.h>
#else
typedef unsigned int pthread_t;
#endif

// -=-= SalData =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
class VCL_DLLPUBLIC X11SalData : public SalData
{
protected:
            BOOL                bNoExceptions_;
            SalXLib            *pXLib_;
            SalDisplay         *m_pSalDisplay;
            pthread_t           hMainThread_;
            rtl::OUString       maLocalHostName;

public:
    X11SalData();
    virtual ~X11SalData();

    virtual void            Init();
    virtual void            initNWF();
    virtual void            deInitNWF();

    inline  void            XError( Display     *pDisplay, XErrorEvent *pEvent ) const;

    SalDisplay*             GetDisplay() const
    { return m_pSalDisplay; }
    void                    SetSalDisplay( SalDisplay* pDisplay )
    { m_pSalDisplay = pDisplay; }

    void                    DeleteDisplay(); // for shutdown

    inline  SalXLib*        GetLib() const { return pXLib_; }
    inline  pthread_t       GetMainThread() const { return hMainThread_; }

    void                    StartTimer( ULONG nMS );
    inline  void            StopTimer();
    void                    Timeout() const;

    const rtl::OUString&    GetLocalHostName() const
    { return maLocalHostName; }

    static int XErrorHdl( Display*, XErrorEvent* );
    static int XIOErrorHdl( Display* );

    // set helper functions to set class and res name in W_CLASS hint
    static const char* getFrameResName();
    static const char* getFrameClassName();
    static rtl::OString getFrameResName( SalExtStyle nStyle );

};

inline X11SalData* GetX11SalData()
{ return (X11SalData*)ImplGetSVData()->mpSalData; }

#ifdef _SV_SALDISP_HXX
inline void X11SalData::XError( Display *pDisplay,  XErrorEvent *pEvent ) const
{ pXLib_->XError( pDisplay, pEvent ); }
#endif

class YieldMutexReleaser
{
    ULONG               m_nYieldCount;
public:
    inline YieldMutexReleaser();
    inline ~YieldMutexReleaser();
};

inline YieldMutexReleaser::YieldMutexReleaser()
{
    m_nYieldCount = GetSalData()->m_pInstance->ReleaseYieldMutex();
}

inline YieldMutexReleaser::~YieldMutexReleaser()
{
    GetSalData()->m_pInstance->AcquireYieldMutex( m_nYieldCount );
}

#endif // _SV_SALDATA_HXX

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>Remove DECLARE_LIST( SalDisplays, SalDisplay* )<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _SV_SALDATA_HXX
#define _SV_SALDATA_HXX

// -=-= includes -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
#include <signal.h>
#include <salstd.hxx>
#include <vcl/salframe.hxx>
#include <salinst.h>
#include <vcl/saldatabasic.hxx>
#include <osl/module.h>
#include <vcl/dllapi.h>

// -=-= forwards -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
class SalXLib;
class SalDisplay;
class SalPrinter;

// -=-= typedefs -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

#if defined LINUX || defined NETBSD || defined AIX || \
    defined FREEBSD || defined OPENBSD || defined DRAGONFLY
#include <pthread.h>
#else
typedef unsigned int pthread_t;
#endif

// -=-= SalData =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
class VCL_DLLPUBLIC X11SalData : public SalData
{
protected:
            BOOL                bNoExceptions_;
            SalXLib            *pXLib_;
            SalDisplay         *m_pSalDisplay;
            pthread_t           hMainThread_;
            rtl::OUString       maLocalHostName;

public:
    X11SalData();
    virtual ~X11SalData();

    virtual void            Init();
    virtual void            initNWF();
    virtual void            deInitNWF();

    inline  void            XError( Display     *pDisplay, XErrorEvent *pEvent ) const;

    SalDisplay*             GetDisplay() const
    { return m_pSalDisplay; }
    void                    SetSalDisplay( SalDisplay* pDisplay )
    { m_pSalDisplay = pDisplay; }

    void                    DeleteDisplay(); // for shutdown

    inline  SalXLib*        GetLib() const { return pXLib_; }
    inline  pthread_t       GetMainThread() const { return hMainThread_; }

    void                    StartTimer( ULONG nMS );
    inline  void            StopTimer();
    void                    Timeout() const;

    const rtl::OUString&    GetLocalHostName() const
    { return maLocalHostName; }

    static int XErrorHdl( Display*, XErrorEvent* );
    static int XIOErrorHdl( Display* );

    // set helper functions to set class and res name in W_CLASS hint
    static const char* getFrameResName();
    static const char* getFrameClassName();
    static rtl::OString getFrameResName( SalExtStyle nStyle );

};

inline X11SalData* GetX11SalData()
{ return (X11SalData*)ImplGetSVData()->mpSalData; }

#ifdef _SV_SALDISP_HXX
inline void X11SalData::XError( Display *pDisplay,  XErrorEvent *pEvent ) const
{ pXLib_->XError( pDisplay, pEvent ); }
#endif

class YieldMutexReleaser
{
    ULONG               m_nYieldCount;
public:
    inline YieldMutexReleaser();
    inline ~YieldMutexReleaser();
};

inline YieldMutexReleaser::YieldMutexReleaser()
{
    m_nYieldCount = GetSalData()->m_pInstance->ReleaseYieldMutex();
}

inline YieldMutexReleaser::~YieldMutexReleaser()
{
    GetSalData()->m_pInstance->AcquireYieldMutex( m_nYieldCount );
}

#endif // _SV_SALDATA_HXX

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<|endoftext|>