bool MessageHandler::validateMsg(SimpleMessage & in)
{
bool rtn = false;
if (in.validateMessage())
{
if (in.getMessageType() == this->getMsgType())
{
rtn = true;
}
else
{
LOG_WARN("Message type: %d, doesn't match handler type: %d",
in.getMessageType(), this->getMsgType());
rtn = false;
}
}
else
{
LOG_WARN("Passed in message invalid");
}
return rtn;
}
bool JointRelayHandler::internalCB(SimpleMessage& msg_in)
{
control_msgs::FollowJointTrajectoryFeedback control_state;
sensor_msgs::JointState sensor_state;
bool rtn = true;
if (create_messages(msg_in, &control_state, &sensor_state))
{
rtn = true;
}
else
rtn = false;
// Reply back to the controller if the sender requested it.
if (CommTypes::SERVICE_REQUEST == msg_in.getMessageType())
{
SimpleMessage reply;
reply.init(msg_in.getMessageType(),
CommTypes::SERVICE_REPLY,
rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE);
this->getConnection()->sendMsg(reply);
}
return rtn;
}
bool PingMessage::init(SimpleMessage & msg)
{
bool rtn = false;
if (this->getMessageType() == msg.getMessageType())
{
rtn = true;
}
else
{
LOG_ERROR("Failed to initialize message, wrong type: %d, expected %d",
msg.getMessageType(), this->getMessageType());
rtn = false;
}
return rtn;
}
TEST(PingMessageSuite, toMessage)
{
PingMessage ping;
SimpleMessage msg;
ping.init();
ASSERT_TRUE(ping.toReply(msg, ReplyTypes::SUCCESS));
EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType());
EXPECT_EQ(CommTypes::SERVICE_REPLY, msg.getCommType());
EXPECT_EQ(ReplyTypes::SUCCESS, msg.getReplyCode());
ASSERT_TRUE(ping.toRequest(msg));
EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType());
EXPECT_EQ(CommTypes::SERVICE_REQUEST, msg.getCommType());
EXPECT_EQ(ReplyTypes::INVALID, msg.getReplyCode());
EXPECT_FALSE(ping.toTopic(msg));
}
