denso_base.cpp

Go to the documentation of this file.

#include <stdlib.h>
#include "denso_robot_core/denso_base.h"
#include "denso_robot_core/denso_variable.h"

#define BCAP_GET_OBJECT_ARGS      (3)
#define BCAP_GET_OBJECTNAMES_ARGS (2)

namespace denso_robot_core {

std::string DensoBase::ConvertBSTRToString(const BSTR bstr)
{
  std::string strRet;
  char *chTmp;

  chTmp = ConvertWideChar2MultiByte(bstr);
  if(chTmp != NULL) {
    strRet = chTmp;
    free(chTmp);
  }

  return strRet;
}

BSTR DensoBase::ConvertStringToBSTR(const std::string& str)
{
  BSTR strRet = NULL;
  wchar_t *chTmp;

  chTmp = ConvertMultiByte2WideChar(str.c_str());
  if(chTmp != NULL) {
    strRet = SysAllocString(chTmp);
    free(chTmp);
  }

  return strRet;
}

std::string DensoBase::RosName() const
{
  std::string tmpName = m_name;

  // Replace space to _
  std::replace(tmpName.begin(), tmpName.end(), ' ', '_');

  // Replace backslash to /
  std::replace(tmpName.begin(), tmpName.end(), '\\', '/');

  // Erase @
  size_t c;
  while((c = tmpName.find_first_of("@")) != std::string::npos) {
    tmpName.erase(c,1);
  }

  // Erase *
  while((c = tmpName.find_first_of("*")) != std::string::npos) {
    tmpName.erase(c,1);
  }

  return tmpName;
}

HRESULT DensoBase::AddVariable(int32_t get_id,
    const std::string& name,
    DensoVariable_Vec& vecVar,
    int16_t vt, bool bRead, bool bWrite, bool bID, int iDuration)
{
  DensoBase_Vec vecBase;
  vecBase.insert(vecBase.end(), vecVar.begin(), vecVar.end());

  if(E_HANDLE == get_Object(vecBase, name, NULL)) {
    Handle_Vec vecHandle;
    HRESULT hr = AddObject(get_id, name, vecHandle);
    if(FAILED(hr)) return hr;

    DensoVariable_Ptr var(new DensoVariable(this,
        m_vecService, vecHandle, name, m_mode,
        vt, bRead, bWrite, bID, iDuration));

    vecVar.push_back(var);
  }

  return S_OK;
}

HRESULT DensoBase::AddVariable(int32_t get_id,
    const XMLElement *xmlVar,
    DensoVariable_Vec& vecVar)
{
  const char *chTmp;

  std::string name;
  int16_t vt = VT_EMPTY;
  bool bRead = false, bWrite = false, bID = false;
  int  iDuration = BCAP_VAR_DEFAULT_DURATION;

  name = xmlVar->GetText();

  chTmp = xmlVar->Attribute(XML_ATTR_VARTYPE);
  if(chTmp != NULL) vt = atoi(chTmp);

  chTmp = xmlVar->Attribute(XML_ATTR_READ);
  if(chTmp != NULL) bRead = (strcasecmp(chTmp, "true") == 0);

  chTmp = xmlVar->Attribute(XML_ATTR_WRITE);
  if(chTmp != NULL) bWrite = (strcasecmp(chTmp, "true") == 0);

  chTmp = xmlVar->Attribute(XML_ATTR_ID);
  if(chTmp != NULL) bID = (strcasecmp(chTmp, "true") == 0);

  chTmp = xmlVar->Attribute(XML_ATTR_DURATION);
  if(chTmp != NULL) iDuration = atoi(chTmp);

  Handle_Vec vecHandle;
  HRESULT hr = AddObject(get_id, name, vecHandle);
  if(FAILED(hr)) return hr;

  DensoVariable_Ptr var(new DensoVariable(this,
      m_vecService, vecHandle, name, m_mode,
      vt, bRead, bWrite, bID, iDuration));

  vecVar.push_back(var);

  return S_OK;
}

HRESULT DensoBase::AddObject(
    int32_t get_id, const std::string& name,
    Handle_Vec& vecHandle)
{
  int srvs, argc;
  HRESULT hr;

  for(srvs = SRV_MIN; srvs <= SRV_MAX; srvs++) {
    VARIANT_Ptr vntRet(new VARIANT());
    VARIANT_Vec vntArgs;

    VariantInit(vntRet.get());

    for(argc = 0; argc < BCAP_GET_OBJECT_ARGS; argc++) {
      VARIANT_Ptr vntTmp(new VARIANT());
      VariantInit(vntTmp.get());

      switch(argc) {
        case 0:
          vntTmp->vt = VT_UI4;
          vntTmp->ulVal = m_vecHandle[srvs];
          break;
        case 1:
          vntTmp->vt = VT_BSTR;
          vntTmp->bstrVal = ConvertStringToBSTR(name);
          break;
        case 2:
          vntTmp->vt = VT_BSTR;
          vntTmp->bstrVal = SysAllocString(L"");
          break;
      }

      vntArgs.push_back(*vntTmp.get());
    }

    hr = m_vecService[srvs]->ExecFunction(get_id, vntArgs, vntRet);
    if(FAILED(hr)) break;

    vecHandle.push_back(vntRet->ulVal);
  }

  return hr;
}

HRESULT DensoBase::GetObjectNames(int32_t func_id, Name_Vec& vecName)
{
  uint32_t argc, i, num;
  HRESULT hr;
  VARIANT_Ptr vntRet(new VARIANT());
  VARIANT_Vec vntArgs;

  VariantInit(vntRet.get());

  for(argc = 0; argc < BCAP_GET_OBJECTNAMES_ARGS; argc++) {
    VARIANT_Ptr vntTmp(new VARIANT());
    VariantInit(vntTmp.get());

    if(argc == 0) {
      vntTmp->vt = VT_UI4;
      vntTmp->ulVal = m_vecHandle[SRV_MIN];
    } else {
      vntTmp->vt = VT_BSTR;
      vntTmp->bstrVal = SysAllocString(L"");
    }

    vntArgs.push_back(*vntTmp.get());
  }

  hr = m_vecService[SRV_MIN]->ExecFunction(func_id, vntArgs, vntRet);
  if(SUCCEEDED(hr)) {
    BSTR    *pbstr;
    VARIANT *pvnt;
    switch(vntRet->vt) {
      case (VT_ARRAY | VT_BSTR):
        num = vntRet->parray->rgsabound->cElements;
        SafeArrayAccessData(vntRet->parray, (void**)&pbstr);
        for(i = 0; i < num; i++) {
          vecName.push_back(ConvertBSTRToString(pbstr[i]));
        }
        SafeArrayUnaccessData(vntRet->parray);
        break;
      case (VT_ARRAY | VT_VARIANT):
        num = vntRet->parray->rgsabound->cElements;
        SafeArrayAccessData(vntRet->parray, (void**)&pvnt);
        for(i = 0; i < num; i++){
          if(pvnt[i].vt != VT_BSTR) {
            hr = E_FAIL;
            break;
          }
          vecName.push_back(ConvertBSTRToString(pvnt[i].bstrVal));
        }
        SafeArrayUnaccessData(vntRet->parray);
        break;
      default:
        hr = S_FALSE;
        break;
    }
  }

  return hr;
}

HRESULT DensoBase::get_Object(const DensoBase_Vec& vecBase,
    int index, DensoBase_Ptr *obj)
{
  try {
    if(obj != NULL) {
      *obj = vecBase.at(index);
    }
  } catch (std::out_of_range&) {
    return E_HANDLE;
  }

  return S_OK;
}

HRESULT DensoBase::get_Object(const DensoBase_Vec& vecBase,
    const std::string& name, DensoBase_Ptr *obj)
{
  DensoBase_Vec::const_iterator it;
  for(it = vecBase.begin(); it != vecBase.end(); it++) {
    if(!strcasecmp((*it)->Name().c_str(), name.c_str())) {
      if(obj != NULL) {
        *obj = *it;
      }
      return S_OK;
    }
  }

  return E_HANDLE;
}

}