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; }
TEST(SimpleMessageSuite, init) { SimpleMessage msg; ByteArray bytes; // Valid messages EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::TOPIC, ReplyTypes::INVALID, bytes)); EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::INVALID, bytes)); EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::SUCCESS, bytes)); EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::FAILURE, bytes)); // Unused command EXPECT_FALSE(msg.init(StandardMsgTypes::INVALID, CommTypes::INVALID,ReplyTypes::INVALID, bytes)); // Service request with a reply EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::SUCCESS, bytes)); EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::FAILURE, bytes)); }
TEST(PingMessageSuite, init) { PingMessage ping; SimpleMessage msg; EXPECT_FALSE(ping.init(msg)); ping.init(); EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType()); ping = PingMessage(); ASSERT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID)); EXPECT_TRUE(ping.init(msg)); EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType()); }
bool SmplMsgConnection::receiveMsg(SimpleMessage & message) { ByteArray lengthBuffer; ByteArray msgBuffer; int length; bool rtn = false; rtn = this->receiveBytes(lengthBuffer, message.getLengthSize()); if (rtn) { rtn = lengthBuffer.unload(length); LOG_COMM("Message length: %d", length); if (rtn) { rtn = this->receiveBytes(msgBuffer, length); if (rtn) { rtn = message.init(msgBuffer); } else { LOG_ERROR("Failed to initialize message"); rtn = false; } } else { LOG_ERROR("Failed to receive message"); rtn = false; } } else { LOG_ERROR("Failed to receive message length"); rtn = false; } return rtn; }