bool GripperMessage::init(industrial::simple_message::SimpleMessage & msg)
{
  bool rtn = false;
  ByteArray data = msg.getData();

  this->setMessageType(LonghornMsgTypes::GRIPPER);
  this->unload(&msg.getData());
  return rtn;

}
bool JointMessage::init(industrial::simple_message::SimpleMessage & msg)
{
  bool rtn = false;
  ByteArray data = msg.getData();

  this->setMessageType(StandardMsgTypes::JOINT_POSITION);

  if (data.unload(this->joints_))
  {
    if (data.unload(this->sequence_))
    {
      rtn = true;
    }
    else
    {
      rtn = false;
      LOG_ERROR("Failed to unload sequence data");
    }
  }
  else
  {
    LOG_ERROR("Failed to unload joint data");
  }
  return rtn;
}
bool TrajectoryDownloadHandler::internalCB(industrial::simple_message::SimpleMessage & in)
{

  bool rtn = false;
  SimpleMessage reply;
  JointTrajPtMessage jMsg;
  
  jMsg.init(in);
  
  switch( jMsg.point_.getSequence() )
  {
  case SpecialSeqValues::START_TRAJECTORY_DOWNLOAD:
    startTrajectory(jMsg);
    rtn = true;
    break;
    
  case SpecialSeqValues::END_TRAJECTORY:
    endTrajectory(jMsg);
    rtn = true;
    break;
    
  case SpecialSeqValues::STOP_TRAJECTORY:
    LOG_DEBUG("Stoping trajectory");
    this->ctrl_->stopMotionJob(JOB_NAME);
    rtn = true;
    break;
    
  default:
    LOG_ERROR("Adding point: %d to trajectory", jMsg.point_.getSequence());
    this->traj_.addPoint(jMsg.point_); 
    break;
  }
  
  // Send ack
  if (CommTypes::SERVICE_REQUEST == in.getCommType())
  {
    if (rtn)
    {
        jMsg.toReply(reply, ReplyTypes::SUCCESS);
    }
    else
    {
        jMsg.toReply(reply, ReplyTypes::FAILURE);
    }
    
    if(this->getConnection()->sendMsg(reply))
    {
        LOG_INFO("Ack sent");
    }
    else
    {
        LOG_ERROR("Failed to send joint ack");
    }
  }
  
  return rtn;
}
Exemple #4
0
bool InputHandler::internalCB(industrial::simple_message::SimpleMessage & in)
{
  bool rtn = false;
  bool unloadStat = false;
  shared_int temp;
  int i = 0;
  SimpleMessage msg;
  
  do
  {
    unloadStat = in.getData().unload(temp);
    LOG_DEBUG("Message data item[%d] = %d", i, temp);
    i++;
  }
  while(unloadStat);
  
  //msg.init(StandardMsgTypes::READ_INPUT, CommTypes::SERVICE_REPLY, ReplyTypes::SUCCESS);
  //rtn = this->getConnection()->sendMsg(msg);
  rtn = true;
  
  return rtn;
}
bool JointMotionHandler::internalCB(industrial::simple_message::SimpleMessage & in)
{
  bool rtn = false;
  JointMessage jMsg;
  SimpleMessage reply;

    if (jMsg.init(in))
    {
        this->motionInterface(jMsg);
    }
    else
    {
    LOG_ERROR("Failed to initialize joint message");
    rtn = false;
    }

    // Send response if requested
    if (CommTypes::SERVICE_REQUEST == in.getCommType())
        if (jMsg.toReply(reply, ReplyTypes::SUCCESS))
        {
            if(this->getConnection()->sendMsg(reply))
            {
                LOG_INFO("Joint ack sent");
                rtn = true;
            }
            else
            {
                LOG_ERROR("Failed to send joint ack");
                rtn = false;
            }
        }
        else
        {
            LOG_ERROR("Failed to generate joint ack message");
            rtn = false;
        }
    
  return rtn;
}