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;
}
示例#2
0
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;
}