denso_robot_rc8.cpp

Go to the documentation of this file.

#include "denso_robot_core/denso_robot_rc8.h"

#define BCAP_ROBOT_EXECUTE_ARGS (3)
#define BCAP_ROBOT_HALT_ARGS    (2)
#define BCAP_ROBOT_MOVE_ARGS    (4)
#define BCAP_ROBOT_SPEED_ARGS   (3)
#define BCAP_ROBOT_CHANGE_ARGS  (2)

#define NAME_MOVESTRING     "_MoveString"
#define NAME_MOVEVALUE      "_MoveValue"
#define NAME_DRIVEEXSTRING  "_DriveExString"
#define NAME_DRIVEEXVALUE   "_DriveExValue"
#define NAME_DRIVEAEXSTRING "_DriveAExString"
#define NAME_DRIVEAEXVALUE  "_DriveAExValue"
#define NAME_SPEED          "_Speed"
#define NAME_CHANGETOOL     "_ChangeTool"
#define NAME_CHANGEWORK     "_ChangeWork"

namespace denso_robot_core {

enum {
  NUM_POSITION = 7,
  NUM_JOINT = 8,
  NUM_TRANS = 10
};

enum {
  ACT_RESET = -1,
  ACT_NONE  =  0,
  ACT_MOVESTRING,
  ACT_MOVEVALUE,
  ACT_DRIVEEXSTRING,
  ACT_DRIVEEXVALUE,
  ACT_DRIVEAEXSTRING,
  ACT_DRIVEAEXVALUE,
};

DensoRobotRC8::DensoRobotRC8(DensoBase* parent,
    Service_Vec& service, Handle_Vec& handle,
    const std::string& name, const int* mode)
  : DensoRobot(parent, service, handle, name, mode),
    m_curAct(ACT_RESET), m_memTimeout(0), m_memRetry(0),
    m_tsfmt(0), m_timestamp(0),
    m_sendfmt(0), m_send_miniio(0), m_send_handio(0),
    m_recvfmt(0), m_recv_miniio(0), m_recv_handio(0),
    m_send_userio_offset(UserIO::MIN_USERIO_OFFSET),
    m_send_userio_size(1),
    m_recv_userio_offset(UserIO::MIN_USERIO_OFFSET),
    m_recv_userio_size(1)
{
  m_tsfmt = TSFMT_MILLISEC;

  m_sendfmt = SENDFMT_MINIIO | SENDFMT_HANDIO;

  m_recvfmt = RECVFMT_POSE_PJ |
      RECVFMT_MINIIO | RECVFMT_HANDIO;
}

DensoRobotRC8::~DensoRobotRC8()
{

}

HRESULT DensoRobotRC8::StartService(ros::NodeHandle& node)
{
  std::string tmpName = DensoBase::RosName();

  if(*m_mode == 0) {
      m_subSpeed = node.subscribe<Float32>(
          tmpName + NAME_SPEED, MESSAGE_QUEUE,
          &DensoRobotRC8::Callback_Speed, this);

      m_subChangeTool = node.subscribe<Int32>(
          tmpName + NAME_CHANGETOOL, MESSAGE_QUEUE,
          boost::bind(&DensoRobotRC8::Callback_Change, this, "Tool", _1));

      m_subChangeWork = node.subscribe<Int32>(
          tmpName + NAME_CHANGEWORK, MESSAGE_QUEUE,
          boost::bind(&DensoRobotRC8::Callback_Change, this, "Work", _1));

      m_actMoveString = boost::make_shared<SimpleActionServer<MoveStringAction> >
        (node, DensoBase::RosName() + NAME_MOVESTRING,
         boost::bind(&DensoRobotRC8::Callback_MoveString, this, _1), false);

      m_actMoveString->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actMoveString->start();

      m_actMoveValue = boost::make_shared<SimpleActionServer<MoveValueAction> >
        (node, DensoBase::RosName() + NAME_MOVEVALUE,
         boost::bind(&DensoRobotRC8::Callback_MoveValue, this, _1), false);

      m_actMoveValue->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actMoveValue->start();

      m_actDriveExString = boost::make_shared<SimpleActionServer<DriveStringAction> >
        (node, DensoBase::RosName() + NAME_DRIVEEXSTRING,
         boost::bind(&DensoRobotRC8::Callback_DriveString, this, "DriveEx", _1), false);

      m_actDriveExString->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actDriveExString->start();

      m_actDriveExValue = boost::make_shared<SimpleActionServer<DriveValueAction> >
        (node, DensoBase::RosName() + NAME_DRIVEEXVALUE,
         boost::bind(&DensoRobotRC8::Callback_DriveValue, this, "DriveEx", _1), false);

      m_actDriveExValue->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actDriveExValue->start();

      m_actDriveAExString = boost::make_shared<SimpleActionServer<DriveStringAction> >
        (node, DensoBase::RosName() + NAME_DRIVEAEXSTRING,
         boost::bind(&DensoRobotRC8::Callback_DriveString, this, "DriveAEx", _1), false);

      m_actDriveAExString->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actDriveAExString->start();

      m_actDriveAExValue = boost::make_shared<SimpleActionServer<DriveValueAction> >
        (node, DensoBase::RosName() + NAME_DRIVEAEXVALUE,
         boost::bind(&DensoRobotRC8::Callback_DriveValue, this, "DriveAEx", _1), false);

      m_actDriveAExValue->registerPreemptCallback(
          boost::bind(&DensoRobotRC8::Callback_Cancel, this));

      m_actDriveAExValue->start();
  }

  DensoRobot::StartService(node);

  m_curAct = ACT_NONE;

  return S_OK;
}

HRESULT DensoRobotRC8::StopService()
{
  DensoRobot::StopService();

  m_mtxAct.lock();
  m_curAct = ACT_RESET;
  m_mtxAct.unlock();

  m_subSpeed.shutdown();
  m_subChangeTool.shutdown();
  m_subChangeWork.shutdown();
  m_actMoveString.reset();
  m_actMoveValue.reset();
  m_actDriveExString.reset();
  m_actDriveExValue.reset();
  m_actDriveAExString.reset();
  m_actDriveAExValue.reset();

  return S_OK;
}

bool DensoRobotRC8::Update()
{
  if(!DensoRobot::Update()) return false;

  boost::mutex::scoped_lock lockSrv(m_mtxSrv);
  if(!m_serving) return false;

  Action_Feedback();

  return true;
}

HRESULT DensoRobotRC8::ExecTakeArm()
{
  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());
  int32_t *pval;

  VariantInit(vntRet.get());

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

    switch(argc) {
      case 0:
        vntTmp->vt = VT_UI4;
        vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
        break;
      case 1:
        vntTmp->vt = VT_BSTR;
        vntTmp->bstrVal = SysAllocString(L"TakeArm");
        break;
      case 2:
        vntTmp->vt = (VT_ARRAY | VT_I4);
        vntTmp->parray = SafeArrayCreateVector(VT_I4, 0, 2);
        SafeArrayAccessData(vntTmp->parray, (void**)&pval);
        pval[0] = m_ArmGroup; // Arm group
        pval[1] = 1L;         // Keep
        SafeArrayUnaccessData(vntTmp->parray);
        break;
    }

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

  return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
}

HRESULT DensoRobotRC8::ExecGiveArm()
{
  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());

  VariantInit(vntRet.get());

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

    switch(argc) {
      case 0:
        vntTmp->vt = VT_UI4;
        vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
        break;
      case 1:
        vntTmp->vt = VT_BSTR;
        vntTmp->bstrVal = SysAllocString(L"GiveArm");
        break;
    }

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

  return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
}

HRESULT DensoRobotRC8::ExecMove(
    int comp, const VARIANT_Ptr& pose, const std::string& option)
{
  HRESULT hr;

  hr = ExecTakeArm();
  if(SUCCEEDED(hr)) {
    int argc;
    VARIANT_Vec vntArgs;
    VARIANT_Ptr vntRet(new VARIANT());

    VariantInit(vntRet.get());

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

      switch(argc) {
        case 0:
          vntTmp->vt = VT_UI4;
          vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
          break;
        case 1:
          vntTmp->vt = VT_I4;
          vntTmp->lVal = comp;
          break;
        case 2:
          VariantCopy(vntTmp.get(), pose.get());
          break;
        case 3:
          vntTmp->vt = VT_BSTR;
          vntTmp->bstrVal = ConvertStringToBSTR(option);
          break;
      }

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

    hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_MOVE, vntArgs, vntRet);

    ExecGiveArm();
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecDrive(
    const std::string& name, const VARIANT_Ptr& option)
{
  HRESULT hr;

  hr = ExecTakeArm();
  if(SUCCEEDED(hr)) {
    int argc;
    VARIANT_Vec vntArgs;
    VARIANT_Ptr vntRet(new VARIANT());

    VariantInit(vntRet.get());

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

      switch(argc) {
        case 0:
          vntTmp->vt = VT_UI4;
          vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
          break;
        case 1:
          vntTmp->vt = VT_BSTR;
          vntTmp->bstrVal = ConvertStringToBSTR(name);
          break;
        case 2:
          VariantCopy(vntTmp.get(), option.get());
          break;
      }

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

    hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);

    ExecGiveArm();
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecSpeed(float value)
{
  HRESULT hr;

  hr = ExecTakeArm();
  if(SUCCEEDED(hr)) {
    int argc;
    VARIANT_Vec vntArgs;
    VARIANT_Ptr vntRet(new VARIANT());

    VariantInit(vntRet.get());

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

      switch(argc) {
        case 0:
          vntTmp->vt = VT_UI4;
          vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
          break;
        case 1:
          vntTmp->vt = VT_I4;
          vntTmp->lVal = -1;
          break;
        case 2:
          vntTmp->vt = VT_R4;
          vntTmp->fltVal = value;
          break;
      }

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

    hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_SPEED, vntArgs, vntRet);

    ExecGiveArm();
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecChange(const std::string& value)
{
  HRESULT hr;

  hr = ExecTakeArm();
  if(SUCCEEDED(hr)) {
    int argc;
    VARIANT_Vec vntArgs;
    VARIANT_Ptr vntRet(new VARIANT());

    VariantInit(vntRet.get());

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

      switch(argc) {
        case 0:
          vntTmp->vt = VT_UI4;
          vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
          break;
        case 1:
          vntTmp->vt = VT_BSTR;
          vntTmp->bstrVal = ConvertStringToBSTR(value);
          break;
      }

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

    hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_CHANGE, vntArgs, vntRet);

    ExecGiveArm();
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecHalt()
{
  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());

  for(argc = 0; argc < BCAP_ROBOT_HALT_ARGS; argc++) {
    VARIANT_Ptr vntTmp(new VARIANT());

    VariantInit(vntTmp.get());

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

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

  return m_vecService[DensoBase::SRV_WATCH]->ExecFunction(ID_ROBOT_HALT, vntArgs, vntRet);
}

HRESULT DensoRobotRC8::ExecCurJnt(std::vector<double>& pose)
{
  HRESULT hr;

  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());

  VariantInit(vntRet.get());

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

    switch(argc) {
      case 0:
        vntTmp->vt = VT_UI4;
        vntTmp->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
        break;
      case 1:
        vntTmp->vt = VT_BSTR;
        vntTmp->bstrVal = SysAllocString(L"HighCurJntEx");
        break;
    }

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

  hr = m_vecService[DensoBase::SRV_WATCH]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);

  if(SUCCEEDED(hr) &&
      (vntRet->vt == (VT_ARRAY | VT_R8)))
  {
    uint32_t num;
    double *pdblval;

    num = vntRet->parray->rgsabound->cElements;
    SafeArrayAccessData(vntRet->parray, (void**)&pdblval);
    pose.resize(num - 1);
    std::copy(&pdblval[1], &pdblval[num], pose.begin());
    SafeArrayUnaccessData(vntRet->parray);
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecSlaveMove(
    const std::vector<double>& pose, std::vector<double>& joint)
{
  HRESULT hr = S_OK;
  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());

  VariantInit(vntRet.get());

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

    switch(argc) {
      case 0:
        vntTmp->vt = VT_UI4;
        vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
        break;
      case 1:
        vntTmp->vt = VT_BSTR;
        vntTmp->bstrVal = SysAllocString(L"slvMove");
        break;
      case 2:
        hr = CreateSendParameter(pose, vntTmp,
            m_send_miniio, m_send_handio,
            m_recv_userio_offset, m_recv_userio_size,
            m_send_userio_offset, m_send_userio_size,
            m_send_userio);
        if(FAILED(hr)) return hr;
        break;
    }

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

  hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
  if(SUCCEEDED(hr)) {
    HRESULT hrTmp = ParseRecvParameter(vntRet,
        m_position, m_joint, m_trans,
        m_recv_miniio, m_recv_handio, m_timestamp,
        m_recv_userio, m_current);

    joint = m_joint;

    if(FAILED(hrTmp)) {
      hr = hrTmp;
    }
  }

  return hr;
}

HRESULT DensoRobotRC8::ChangeMode(int mode)
{
  HRESULT hr = S_OK;

  if(*m_mode == 0) {
    // Change to slave mode
    if(mode != 0) {
      hr = ExecSlaveMode("slvSendFormat", m_sendfmt);
      if(FAILED(hr)) return hr;

      hr = ExecSlaveMode("slvRecvFormat", m_recvfmt, m_tsfmt);
      if(FAILED(hr)) return hr;

      hr = ExecTakeArm();
      if(FAILED(hr)) return hr;

      hr = ExecSlaveMode("slvChangeMode", mode);
      if(FAILED(hr)) return hr;

      m_memTimeout = m_vecService[DensoBase::SRV_ACT]->get_Timeout();
      m_memRetry   = m_vecService[DensoBase::SRV_ACT]->get_Retry();
      m_vecService[DensoBase::SRV_ACT]->put_Timeout(8);
      m_vecService[DensoBase::SRV_ACT]->put_Retry(3);
    }
  } else {
    m_vecService[DensoBase::SRV_ACT]->put_Timeout(m_memTimeout);
    m_vecService[DensoBase::SRV_ACT]->put_Retry(m_memRetry);

    hr = ExecSlaveMode("slvChangeMode", mode);
    ExecGiveArm();
  }

  return hr;
}

HRESULT DensoRobotRC8::ExecSlaveMode(
    const std::string& name, int32_t format, int32_t option)
{
  int argc;
  VARIANT_Vec vntArgs;
  VARIANT_Ptr vntRet(new VARIANT());
  int32_t *pval;

  VariantInit(vntRet.get());

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

    switch(argc) {
      case 0:
        vntTmp->vt = VT_UI4;
        vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
        break;
      case 1:
        vntTmp->vt = VT_BSTR;
        vntTmp->bstrVal = ConvertStringToBSTR(name);
        break;
      case 2:
        if(option == 0) {
            vntTmp->vt = VT_I4;
            vntTmp->lVal = format;
        } else {
            vntTmp->vt = (VT_ARRAY | VT_I4);
            vntTmp->parray = SafeArrayCreateVector(VT_I4, 0, 2);
            SafeArrayAccessData(vntTmp->parray, (void**)&pval);
            pval[0] = format;
            pval[1] = option;
            SafeArrayUnaccessData(vntTmp->parray);
        }
        break;
    }

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

  return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
}

HRESULT DensoRobotRC8::CreateSendParameter(
    const std::vector<double>& pose, VARIANT_Ptr& send,
    const int miniio, const int handio,
    const int recv_userio_offset, const int recv_userio_size,
    const int send_userio_offset, const int send_userio_size,
    const std::vector<uint8_t>& send_userio)
{
  int type = *m_mode & SLVMODE_POSE;

  // Check pose type
  int joints = 0;
  switch(type) {
    case SLVMODE_POSE_P:
      joints = NUM_POSITION;
      break;
    case SLVMODE_POSE_J:
      joints = NUM_JOINT;
      break;
    case SLVMODE_POSE_T:
      joints = NUM_TRANS;
      break;
    default:
      return E_FAIL;
  }

  if(joints < pose.size()) {
    return E_FAIL;
  }

  // Check send format
  bool send_hio, send_mio, send_uio, recv_uio;
  send_hio = m_sendfmt & SENDFMT_HANDIO;
  send_mio = m_sendfmt & SENDFMT_MINIIO;
  send_uio = m_sendfmt & SENDFMT_USERIO;

  if(send_uio) {
    if(send_userio_size < send_userio.size()) {
      return E_FAIL;
    }
  }

  // Check receive format
  recv_uio = m_recvfmt & RECVFMT_USERIO;

  // Number of arguments
  int num = 1 + send_hio + send_mio + 3 * send_uio + 2 * recv_uio;

  VARIANT *pvnt;
  double *pdbl;
  uint8_t *pbool;

  if(num == 1) {
    // Pose only
    send->vt = (VT_ARRAY | VT_R8);
    send->parray = SafeArrayCreateVector(VT_R8, 0, joints);
    SafeArrayAccessData(send->parray, (void**)&pdbl);
    memset(pdbl, 0, joints * sizeof(double));
    std::copy(pose.begin(), pose.end(), pdbl);
    SafeArrayUnaccessData(send->parray);
  } else {
    send->vt = (VT_ARRAY | VT_VARIANT);
    send->parray = SafeArrayCreateVector(VT_VARIANT, 0, num);

    SafeArrayAccessData(send->parray, (void**)&pvnt);

    int offset = 0;

    // Pose
    {
      pvnt[offset].vt = (VT_ARRAY | VT_R8);
      pvnt[offset].parray = SafeArrayCreateVector(VT_R8, 0, joints);
      SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
      memset(pdbl, 0, joints * sizeof(double));
      std::copy(pose.begin(), pose.end(), pdbl);
      SafeArrayUnaccessData(pvnt[offset].parray);

      offset++;
    }

    // Mini I/O
    if(send_mio) {
      pvnt[offset].vt = VT_I4;
      pvnt[offset].lVal = miniio;

      offset++;
    }

    // Send User I/O
    if(send_uio) {
      pvnt[offset+0].vt = VT_I4;
      pvnt[offset+0].lVal = send_userio_offset;
      
      pvnt[offset+1].vt = VT_I4;
      pvnt[offset+1].lVal = send_userio_size * UserIO::USERIO_ALIGNMENT;

      pvnt[offset+2].vt = (VT_ARRAY | VT_UI1);
      pvnt[offset+2].parray = SafeArrayCreateVector(VT_UI1, 0, send_userio_size);
      SafeArrayAccessData(pvnt[offset+2].parray, (void**)&pbool);
      memset(pbool, 0, send_userio_size * sizeof(uint8_t));
      std::copy(send_userio.begin(), send_userio.end(), pbool);
      SafeArrayUnaccessData(pvnt[offset+2].parray);

      offset+=3;
    }

    // Receive User I/O
    if(recv_uio) {
      pvnt[offset+0].vt = VT_I4;
      pvnt[offset+0].lVal = recv_userio_offset;

      pvnt[offset+1].vt = VT_I4;
      pvnt[offset+1].lVal = recv_userio_size * UserIO::USERIO_ALIGNMENT;

      offset+=2;
    }

    // Hand I/O
    if(send_hio) {
      pvnt[offset].vt = VT_I4;
      pvnt[offset].lVal = handio;

      offset++;
    }

    SafeArrayUnaccessData(send->parray);
  }

  return S_OK;
}

HRESULT DensoRobotRC8::ParseRecvParameter(
    const VARIANT_Ptr& recv,
    std::vector<double>& position, std::vector<double>& joint, std::vector<double>& trans,
    int& miniio, int& handio, int& timestamp,
    std::vector<uint8_t>& recv_userio,
    std::vector<double>& current)
{
  int type = m_recvfmt & SLVMODE_POSE;

  // Check pose type
  int j = 0, j1 = 0, j2 = 0, joints = 0;
  std::vector<double> *pose1 = NULL, *pose2 = NULL;

  switch(type) {
    case RECVFMT_POSE_P:
      j1 = NUM_POSITION;
      pose1 = &position;
      break;
    case RECVFMT_POSE_J:
      j1 = NUM_JOINT;
      pose1 = &joint;
      break;
    case RECVFMT_POSE_T:
      j1 = NUM_TRANS;
      pose1 = &trans;
      break;
    case RECVFMT_POSE_PJ:
      j1 = NUM_POSITION;
      j2 = NUM_JOINT;
      pose1 = &position;
      pose2 = &joint;
      break;
    case RECVFMT_POSE_TJ:
      j1 = NUM_TRANS;
      j2 = NUM_JOINT;
      pose1 = &trans;
      pose2 = &joint;
      break;
    default:
      return E_FAIL;
  }

  joints = j1 + j2;

  // Check receive format
  bool recv_ts, recv_hio, recv_mio, recv_uio, recv_crt;
  recv_ts  = m_recvfmt & RECVFMT_TIME;
  recv_hio = m_recvfmt & RECVFMT_HANDIO;
  recv_mio = m_recvfmt & RECVFMT_MINIIO;
  recv_uio = m_recvfmt & RECVFMT_USERIO;
  recv_crt = m_recvfmt & RECVFMT_CURRENT;

  // Number of arguments
  int num = 1 + recv_ts + recv_hio + recv_mio + recv_uio + recv_crt;

  HRESULT hr = S_OK;
  VARIANT *pvnt;
  double *pdbl;
  uint8_t *pbool;

  if(recv->vt == (VT_ARRAY | VT_R8)) {
    if(joints != recv->parray->rgsabound->cElements) {
      return E_FAIL;
    }

    // Pose only
    SafeArrayAccessData(recv->parray, (void**)&pdbl);
    pose1->resize(j1);
    std::copy(pdbl, &pdbl[j1], pose1->begin());
    if(pose2 != NULL) {
      pose2->resize(j2);
      std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
    }
    SafeArrayUnaccessData(recv->parray);
  }
  else if(recv->vt == (VT_ARRAY | VT_VARIANT)) {
    if(num != recv->parray->rgsabound->cElements) {
      return E_FAIL;
    }

    SafeArrayAccessData(recv->parray, (void**)&pvnt);

    int offset = 0;

    // Timestamp
    if(recv_ts) {
      if(pvnt[offset].vt != VT_I4) {
        hr = E_FAIL;
        goto exit_proc;
      }

      timestamp = pvnt[offset].lVal;

      offset++;
    }

    // Pose
    {
      if((pvnt[offset].vt != (VT_ARRAY | VT_R8))
          || (joints != pvnt[offset].parray->rgsabound->cElements))
      {
        hr = E_FAIL;
        goto exit_proc;
      }

      SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
      pose1->resize(j1);
      std::copy(pdbl, &pdbl[j1], pose1->begin());
      if(pose2 != NULL) {
        pose2->resize(j2);
        std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
      }
      SafeArrayUnaccessData(pvnt[offset].parray);

      offset++;
    }

    // Mini I/O
    if(recv_mio) {
      if(pvnt[offset].vt != VT_I4) {
        hr = E_FAIL;
        goto exit_proc;
      }

      miniio = pvnt[offset].lVal;

      offset++;
    }

    // User I/O
    if(recv_uio) {
      if(pvnt[offset].vt != (VT_ARRAY | VT_UI1))
      {
        hr = E_FAIL;
        goto exit_proc;
      }

      SafeArrayAccessData(pvnt[offset].parray, (void**)&pbool);
      recv_userio.resize(pvnt[offset].parray->rgsabound->cElements);
      std::copy(pbool, &pbool[pvnt[offset].parray->rgsabound->cElements], recv_userio.begin());
      SafeArrayUnaccessData(pvnt[offset].parray);

      offset++;
    }

    // Hand I/O
    if(recv_hio) {
      if(pvnt[offset].vt != VT_I4) {
        hr = E_FAIL;
        goto exit_proc;
      }

      handio = pvnt[offset].lVal;

      offset++;
    }

    // Current
    if(recv_crt) {
      if((pvnt[offset].vt != (VT_ARRAY | VT_R8))
          || (8 != pvnt[offset].parray->rgsabound->cElements))
      {
        hr = E_FAIL;
        goto exit_proc;
      }

      SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
      current.resize(8);
      std::copy(pdbl, &pdbl[8], current.begin());
      SafeArrayUnaccessData(pvnt[offset].parray);

      offset++;
    }

exit_proc:
    SafeArrayUnaccessData(recv->parray);
  } else {
    return E_FAIL;
  }

  return hr;
}

void DensoRobotRC8::Callback_MoveString(const MoveStringGoalConstPtr& goal)
{
  HRESULT hr;
  MoveStringResult res;

  // Set current action
  boost::mutex::scoped_lock lockAct(m_mtxAct);
  if(m_curAct != ACT_NONE) {
    if(m_curAct != ACT_RESET) {
      res.HRESULT = E_FAIL;
      m_actMoveString->setAborted(res);
    }
    return;
  }
  m_curAct = ACT_MOVESTRING;
  lockAct.unlock();

  VARIANT_Ptr vntPose(new VARIANT());
  VariantInit(vntPose.get());
  vntPose->vt = VT_BSTR;
  vntPose->bstrVal = ConvertStringToBSTR(goal->pose);

  hr = ExecMove(goal->comp, vntPose, goal->option);

  // Reset current action
  m_mtxAct.lock();
  if(m_curAct == ACT_MOVESTRING) {
    if(SUCCEEDED(hr)) {
      res.HRESULT = S_OK;
      m_actMoveString->setSucceeded(res);
    } else {
      res.HRESULT = hr;
      m_actMoveString->setAborted(res);
    }
    m_curAct = ACT_NONE;
  }
  m_mtxAct.unlock();
}

void DensoRobotRC8::Callback_MoveValue(const MoveValueGoalConstPtr& goal)
{
  HRESULT hr;
  MoveValueResult res;

  // Set current action
  boost::mutex::scoped_lock lockAct(m_mtxAct);
  if(m_curAct != ACT_NONE) {
    if(m_curAct != ACT_RESET) {
      res.HRESULT = E_FAIL;
      m_actMoveValue->setAborted(res);
    }
    return;
  }
  m_curAct = ACT_MOVEVALUE;
  lockAct.unlock();

  VARIANT_Ptr vntPose(new VARIANT());
  VariantInit(vntPose.get());
  CreatePoseData(goal->pose, *vntPose.get());

  hr = ExecMove(goal->comp, vntPose, goal->option);

  // Reset current action
  m_mtxAct.lock();
  if(m_curAct == ACT_MOVEVALUE) {
    if(SUCCEEDED(hr)){
      res.HRESULT = S_OK;
      m_actMoveValue->setSucceeded(res);
    }else{
      res.HRESULT = hr;
      m_actMoveValue->setAborted(res);
    }
    m_curAct = ACT_NONE;
  }
  m_mtxAct.unlock();
}

void DensoRobotRC8::Callback_DriveString(
    const std::string& name, const DriveStringGoalConstPtr& goal)
{
  HRESULT hr;
  DriveStringResult res;
  BSTR *pbstr;

  int act = 0;
  boost::shared_ptr<SimpleActionServer<DriveStringAction> > actSvr;

  if(!name.compare("DriveEx")) {
    act    = ACT_DRIVEEXSTRING;
    actSvr = m_actDriveExString;
  }
  else if(!name.compare("DriveAEx")) {
    act    = ACT_DRIVEAEXSTRING;
    actSvr = m_actDriveAExString;
  }
  else return;

  // Set current action
  boost::mutex::scoped_lock lockAct(m_mtxAct);
  if(m_curAct != ACT_NONE) {
    if(m_curAct != ACT_RESET) {
      res.HRESULT = E_FAIL;
      actSvr->setAborted(res);
    }
    return;
  }
  m_curAct = act;
  lockAct.unlock();

  VARIANT_Ptr vntOpt(new VARIANT());
  VariantInit(vntOpt.get());
  vntOpt->vt = (VT_ARRAY | VT_BSTR);
  vntOpt->parray = SafeArrayCreateVector(VT_BSTR, 0, 2);
  SafeArrayAccessData(vntOpt->parray, (void**)&pbstr);
  pbstr[0] = ConvertStringToBSTR(goal->pose);
  pbstr[1] = ConvertStringToBSTR(goal->option);
  SafeArrayUnaccessData(vntOpt->parray);

  hr = ExecDrive(name, vntOpt);

  // Reset current action
  m_mtxAct.lock();
  if(m_curAct == act) {
    if(SUCCEEDED(hr)) {
      res.HRESULT = S_OK;
      actSvr->setSucceeded(res);
    } else {
      res.HRESULT = hr;
      actSvr->setAborted(res);
    }
    m_curAct = ACT_NONE;
  }
  m_mtxAct.unlock();
}

void DensoRobotRC8::Callback_DriveValue(
    const std::string& name, const DriveValueGoalConstPtr& goal)
{
  HRESULT  hr;
  DriveValueResult res;
  VARIANT *pvntval, *pvntjnt;

  int act = 0;
  boost::shared_ptr<SimpleActionServer<DriveValueAction> > actSvr;

  if(!name.compare("DriveEx")) {
    act    = ACT_DRIVEEXVALUE;
    actSvr = m_actDriveExValue;
  }
  else if(!name.compare("DriveAEx")) {
    act    = ACT_DRIVEAEXVALUE;
    actSvr = m_actDriveAExValue;
  }
  else return;

  // Set current action
  boost::mutex::scoped_lock lockAct(m_mtxAct);
  if(m_curAct != ACT_NONE) {
    if(m_curAct != ACT_RESET) {
      res.HRESULT = E_FAIL;
      actSvr->setAborted(res);
    }
    return;
  }
  m_curAct = act;
  lockAct.unlock();

  VARIANT_Ptr vntOpt(new VARIANT());
  VariantInit(vntOpt.get());

  vntOpt->vt = (VT_ARRAY | VT_VARIANT);
  vntOpt->parray = SafeArrayCreateVector(VT_VARIANT, 0, 2);

  SafeArrayAccessData(vntOpt->parray, (void**)&pvntval);

  pvntval[0].vt = (VT_ARRAY | VT_VARIANT);
  pvntval[0].parray = SafeArrayCreateVector(VT_VARIANT, 0, goal->pose.size());

  SafeArrayAccessData(pvntval[0].parray, (void**)&pvntjnt);

  for(int i = 0; i < goal->pose.size(); i++) {
    PoseData pd;
    pd.value.push_back(goal->pose.at(i).joint);
    pd.value.push_back(goal->pose.at(i).value);
    pd.type = -1;
    pd.pass = (i == 0) ? goal->pass : 0;
    CreatePoseData(pd, pvntjnt[i]);
  }

  SafeArrayUnaccessData(pvntval[0].parray);

  pvntval[1].vt = VT_BSTR;
  pvntval[1].bstrVal = ConvertStringToBSTR(goal->option);

  SafeArrayUnaccessData(vntOpt->parray);

  hr = ExecDrive(name, vntOpt);

  // Reset current action
  m_mtxAct.lock();
  if(m_curAct == act) {
    if(SUCCEEDED(hr)) {
      res.HRESULT = S_OK;
      actSvr->setSucceeded(res);
    }else{
      res.HRESULT = hr;
      actSvr->setAborted(res);
    }
    m_curAct = ACT_NONE;
  }
  m_mtxAct.unlock();
}

void DensoRobotRC8::Callback_Speed(const Float32::ConstPtr& msg)
{
  ExecSpeed(msg->data);
}

void DensoRobotRC8::Callback_Change(
    const std::string& name, const Int32::ConstPtr& msg)
{
  std::stringstream ss;
  ss << name << msg->data;
  ExecChange(ss.str());
}

void DensoRobotRC8::Callback_Cancel()
{
  boost::mutex::scoped_lock lockAct(m_mtxAct);

  if(m_curAct > ACT_NONE) {
    ExecHalt();

    switch(m_curAct){
      case ACT_MOVESTRING:
        m_actMoveString->setPreempted();
        break;
      case ACT_MOVEVALUE:
        m_actMoveValue->setPreempted();
        break;
      case ACT_DRIVEEXSTRING:
        m_actDriveExString->setPreempted();
        break;
      case ACT_DRIVEEXVALUE:
        m_actDriveExValue->setPreempted();
        break;
      case ACT_DRIVEAEXSTRING:
        m_actDriveAExString->setPreempted();
        break;
      case ACT_DRIVEAEXVALUE:
        m_actDriveAExValue->setPreempted();
        break;
    }

    m_curAct = ACT_NONE;
  }
}

void DensoRobotRC8::Action_Feedback()
{
  boost::mutex::scoped_lock lockAct(m_mtxAct);

  if(m_curAct > ACT_NONE) {
    HRESULT hr;
    std::vector<double> pose;

    MoveStringFeedback  fbMvStr;
    MoveValueFeedback   fbMvVal;
    DriveStringFeedback fbDrvStr;
    DriveValueFeedback  fbDrvVal;

    hr = ExecCurJnt(pose);

    if(SUCCEEDED(hr)) {
      switch(m_curAct){
        case ACT_MOVESTRING:
          fbMvStr.pose = pose;
          m_actMoveString->publishFeedback(fbMvStr);
          break;
        case ACT_MOVEVALUE:
          fbMvVal.pose = pose;
          m_actMoveValue->publishFeedback(fbMvVal);
          break;
        case ACT_DRIVEEXSTRING:
          fbDrvStr.pose = pose;
          m_actDriveExString->publishFeedback(fbDrvStr);
          break;
        case ACT_DRIVEEXVALUE:
          fbDrvVal.pose = pose;
          m_actDriveExValue->publishFeedback(fbDrvVal);
          break;
        case ACT_DRIVEAEXSTRING:
          fbDrvStr.pose = pose;
          m_actDriveAExString->publishFeedback(fbDrvStr);
          break;
        case ACT_DRIVEAEXVALUE:
          fbDrvVal.pose = pose;
          m_actDriveAExValue->publishFeedback(fbDrvVal);
          break;
      }
    }
  }
}

int DensoRobotRC8::get_SendFormat() const
{
  return m_sendfmt;
}

void DensoRobotRC8::put_SendFormat(int format)
{
  switch(format) {
    case SENDFMT_NONE:
    case SENDFMT_HANDIO:
    case SENDFMT_MINIIO:
    case SENDFMT_HANDIO | SENDFMT_MINIIO:
    case SENDFMT_USERIO:
    case SENDFMT_USERIO | SENDFMT_HANDIO:
      m_sendfmt = format;
      break;
    default:
      ROS_WARN("Failed to put_SendFormat.");
      break;
  }
}
  
int DensoRobotRC8::get_RecvFormat() const
{
  return m_recvfmt;
}
  
void DensoRobotRC8::put_RecvFormat(int format)
{
  int pose = format & RECVFMT_POSE;
  if((RECVFMT_NONE <= pose)
     && (pose <= RECVFMT_POSE_TJ))
  {
    switch(format & ~RECVFMT_POSE) {
      case RECVFMT_NONE:
      case RECVFMT_TIME:
      case RECVFMT_HANDIO:
      case RECVFMT_CURRENT:
      case RECVFMT_MINIIO:
      case RECVFMT_USERIO:
      case RECVFMT_TIME | RECVFMT_HANDIO:
      case RECVFMT_TIME | RECVFMT_MINIIO:
      case RECVFMT_HANDIO | RECVFMT_MINIIO:
      case RECVFMT_TIME | RECVFMT_HANDIO | RECVFMT_MINIIO:
      case RECVFMT_TIME | RECVFMT_USERIO:
      case RECVFMT_HANDIO | RECVFMT_USERIO:
      case RECVFMT_TIME | RECVFMT_HANDIO | RECVFMT_USERIO:
      case RECVFMT_CURRENT | RECVFMT_MINIIO:
      case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_MINIIO:
      case RECVFMT_CURRENT | RECVFMT_HANDIO:
      case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO:
      case RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_MINIIO:
      case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_MINIIO:
      case RECVFMT_CURRENT | RECVFMT_USERIO:
      case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_USERIO:
      case RECVFMT_CURRENT | RECVFMT_MINIIO | RECVFMT_USERIO:
      case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_USERIO:
        m_recvfmt = format;
        break;
      default:
        ROS_WARN("Failed to put_RecvFormat.");
        break;
    }
  }
  else
  {
    ROS_WARN("Failed to put_RecvFormat.");
  }
}

int DensoRobotRC8::get_TimeFormat() const
{
  return m_tsfmt;
}

void DensoRobotRC8::put_TimeFormat(int format)
{
  if((format == TSFMT_MILLISEC)
     || (format == TSFMT_MICROSEC))
  {
    m_tsfmt = format;
  }
  else
  {
    ROS_WARN("Failed to put_TimeFormat.");
  }
}

int DensoRobotRC8::get_MiniIO() const
{
  return m_recv_miniio;
}

void DensoRobotRC8::put_MiniIO(int value)
{
  m_send_miniio = value;
}

int DensoRobotRC8::get_HandIO() const
{
  return m_recv_handio;
}

void DensoRobotRC8::put_HandIO(int value)
{
  m_send_handio = value;
}

void DensoRobotRC8::put_SendUserIO(const UserIO& value)
{
  if(value.offset < UserIO::MIN_USERIO_OFFSET)
  {
    ROS_WARN("User I/O offset has to be greater than %d.",
      UserIO::MIN_USERIO_OFFSET - 1);
    return;
  }
  
  if(value.offset % UserIO::USERIO_ALIGNMENT)
  {
    ROS_WARN("User I/O offset has to be multiple of %d.",
      UserIO::USERIO_ALIGNMENT);
    return;
  }
  
  if(value.size <= 0)
  {
    ROS_WARN("User I/O size has to be greater than 0.");
    return;
  }
  
  if(value.size < value.value.size())
  {
    ROS_WARN("User I/O size has to be equal or greater than the value length.");
    return;
  }
  
  m_send_userio_offset = value.offset;
  m_send_userio_size = value.size;
  m_send_userio = value.value;
}

void DensoRobotRC8::put_RecvUserIO(const UserIO& value)
{
  if(value.offset < UserIO::MIN_USERIO_OFFSET)
  {
    ROS_WARN("User I/O offset has to be greater than %d.",
      UserIO::MIN_USERIO_OFFSET - 1);
    return;
  }
  
  if(value.offset % UserIO::USERIO_ALIGNMENT)
  {
    ROS_WARN("User I/O offset has to be multiple of %d.",
      UserIO::USERIO_ALIGNMENT);
    return;
  }
  
  if(value.size <= 0)
  {
    ROS_WARN("User I/O size has to be greater than 0.");
    return;
  }

  m_recv_userio_offset = value.offset;
  m_recv_userio_size = value.size;
}

void DensoRobotRC8::get_RecvUserIO(UserIO& value) const
{
  value.offset = m_recv_userio_offset;
  value.size = m_recv_userio.size();
  value.value = m_recv_userio;
}

void DensoRobotRC8::get_Current(std::vector<double> &current) const
{
  current = m_current;
}

int DensoRobotRC8::get_Timestamp() const
{
  return m_timestamp;
}
  
}