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