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;
}
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;
}