void JointMotionHandler::motionInterface(JointMessage & jMsg)
{
//
// Upon receiving...
// 1st point - initialize buffer, enable motion, start job, add point (increment buffer index),
// Nth point - add point (increment buffer index)
// end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
//motion stop - disable motion, stop job
JointData joints;
switch (jMsg.getSequence())
{
case SpecialSeqValues::END_TRAJECTORY:
LOG_INFO("Received end trajectory command");
while(!pVarQ.bufferEmpty())
{
LOG_DEBUG("Waiting for motion buffer to empty");
mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
};
this->stopMotionJob();
break;
case SpecialSeqValues::STOP_TRAJECTORY:
LOG_WARN("Received stop command, stopping motion immediately");
this->stopMotionJob();
break;
default:
joints.copyFrom(jMsg.getJoints());
if (!(this->isJobStarted()))
{
//TODO: The velocity should be set from the message in the future.
pVarQ.init(joints, 0.0);
this->startMotionJob();
}
else
{
pVarQ.addPoint(joints, 0.0);
}
}
}
bool internalCB(industrial::simple_message::SimpleMessage & in)
{
bool rtn = false;
JointMessage jm;
SimpleMessage msg;
if (jm.init(in))
{
ROS_INFO_STREAM("Received sequence number: " << jm.getSequence());
if (jm.toReply(msg, ReplyTypes::SUCCESS))
{
if (this->getConnection()->sendMsg(msg))
{
ROS_INFO_STREAM("Sending reply code: " << msg.getReplyCode());
rtn = true;
}
else
{
ROS_ERROR("Failed to send joint message return");
rtn = false;
}
}
else
{
ROS_ERROR("Failed to generate joint reply message");
rtn = false;
}
}
else
{
ROS_ERROR("Failed to initialize joint message");
rtn = false;
}
return rtn;
}