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 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;
}
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 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;
}
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;
}
void NetworkEventStrategy::processCategoryList(SimpleMessage &message) const {
LOG(INFO) << "NetworkEventStrategy::processCategoryList:\n" << message.toString();
CategoryListMessage& msg = dynamic_cast<CategoryListMessage&>(message);
if (controller->getState() == Controller::CATEGORY_LIST) {
controller->incrementServerResponseNo();
getView()->showCategoryList(msg.getCategories());
showMainMenu();
} else if (controller->getState() == Controller::SIGN_UP) {
controller->incrementServerResponseNo();
getView()->showSignUpCategorySubMenu(filterCategories(msg.getCategories()));
}
}
// Trivial equality operators to enable gtest macros.
bool operator==(const SimpleMessage& left, const SimpleMessage& right)
{
if (left.id() != right.id() ||
left.numbers().size() != right.numbers().size()) {
return false;
}
return std::equal(
left.numbers().begin(), left.numbers().end(), right.numbers().begin());
}
bool SmplMsgConnection::sendMsg(SimpleMessage & message)
{
bool rtn;
ByteArray sendBuffer;
ByteArray msgData;
if (message.validateMessage())
{
message.toByteArray(msgData);
sendBuffer.load((int)msgData.getBufferSize());
sendBuffer.load(msgData);
rtn = this->sendBytes(sendBuffer);
}
else
{
rtn = false;
LOG_ERROR("Message validation failed, message not sent");
}
return rtn;
}
void NetworkEventStrategy::processNeighbourSet(SimpleMessage &message) const {
LOG(INFO) << "NetworkEventStrategy::processNeighbourSet:\n" << message.toString();
NeighboursInfoMessage& msg = dynamic_cast<NeighboursInfoMessage&>(message);
getModel()->updateLeftNeighbour(msg.getCategoryId(),
ConnectionInfo(msg.getLeftNeighbourIP(), msg.getLeftNeighbourPort(),
msg.getLeftNeighbourName()));
stringstream ss;
ss << "Neighbours updated: \n";
ss << "Left neighbour: " << msg.getLeftNeighbourName() << endl;
ss << "Right neighbour: " << msg.getRightNeighbourName();
getModel()->addNotification(ss.str());
}
bool MotionReplyMessage::init(SimpleMessage & msg)
{
ByteArray data = msg.getData();
this->init();
if (!data.unload(this->reply_))
{
LOG_ERROR("Failed to unload MotionReplyMessage data");
return false;
}
return true;
}
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;
}
void NetworkEventStrategy::processRingMessage(SimpleMessage &message) const {
LOG(INFO) << "NetworkEventStrategy::processRingMessage:\n" << message.toString();
RingMessage& msg = dynamic_cast<RingMessage&>(message);
if (!getModel()->isMyCategory(msg.getCategoryId())) {
getModel()->addMessageToInbox(msg);
getModel()->addNotification("You have a new message!\nCheck your inbox.");
} else {
stringstream ss;
ss << "Your message in " << getModel()->getCategoryName(msg.getCategoryId()) << " category has been read by:";
for (string name : msg.getConfirmationsList()) {
ss << endl << name;
}
getModel()->addNotification(ss.str());
}
}
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));
}
void NetworkEventStrategy::processErrorMessage(SimpleMessage &message) const {
LOG(INFO) << "NetworkEventStrategy::processRingMessage:\n" << message.toString();
NetworkControllerErrorMessage &msg = dynamic_cast<NetworkControllerErrorMessage &>(message);
switch (msg.getErrorCode()) {
case NetworkControllerErrorMessage::UNABLE_TO_CREATE_LISTENING_SOCKET:
getView()->showInfo("ERROR!\nCan't create connection! You should try to use another settings.");
showMainMenu();
break;
case NetworkControllerErrorMessage::UNABLE_TO_SEND_MSG:
getModel()->addNotification("ERROR!\nCan't send message!");
break;
case NetworkControllerErrorMessage::UNABLE_TO_RESERIALIZE_MSG:
getModel()->addNotification("ERROR!\nCan't read received message!");
break;
case NetworkControllerErrorMessage::UNABLE_TO_SERIALIZE_MSG:
getModel()->addNotification("ERROR!\nProblem with sending message occured!");
break;
case NetworkControllerErrorMessage::UNDEFINED:
getModel()->addNotification("ERROR!\nUndefined problem with connection occured!");
break;
}
}
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));
}
void goalCB(GoalHandle gh)
{
GripperMessage gMsg;
SimpleMessage request;
SimpleMessage reply;
ROS_DEBUG("Received grasping goal");
while (!(robot_->isConnected()))
{
ROS_DEBUG("Reconnecting");
robot_->makeConnect();
}
switch(gh.getGoal()->goal)
{
case GraspHandPostureExecutionGoal::PRE_GRASP:
gh.setAccepted();
ROS_WARN("Pre-grasp is not supported by this gripper");
gh.setSucceeded();
break;
case GraspHandPostureExecutionGoal::GRASP:
case GraspHandPostureExecutionGoal::RELEASE:
gh.setAccepted();
switch(gh.getGoal()->goal)
{
case GraspHandPostureExecutionGoal::GRASP:
ROS_INFO("Executing a gripper grasp");
gMsg.init(GripperOperationTypes::CLOSE);
break;
case GraspHandPostureExecutionGoal::RELEASE:
ROS_INFO("Executing a gripper release");
gMsg.init(GripperOperationTypes::OPEN);
break;
}
gMsg.toRequest(request);
this->robot_->sendAndReceiveMsg(request, reply);
switch(reply.getReplyCode())
{
case ReplyTypes::SUCCESS:
ROS_INFO("Robot gripper returned success");
gh.setSucceeded();
break;
case ReplyTypes::FAILURE:
ROS_ERROR("Robot gripper returned failure");
gh.setCanceled();
break;
}
break;
default:
gh.setRejected();
break;
}
}
void NetworkEventStrategy::processServerInfo(SimpleMessage &message) const {
LOG(INFO) << "NetworkEventStrategy::processServerInfo:\n" << message.toString();
ServerInfoMessage& msg = dynamic_cast<ServerInfoMessage&>(message);
switch (msg.getInfoType()){
case ServerInfoMessageType::CATEGORY_CREATED :
controller->incrementServerResponseNo();
getModel()->addMyCategory(msg.getExtraInfo(), msg.getInfo());
getModel()->addNotification("New category created!");
showMainMenu();
break;
case ServerInfoMessageType::CATEGORY_REMOVED :
controller->incrementServerResponseNo();
getModel()->removeCategoryAndData(msg.getExtraInfo());
getModel()->addNotification("One of your category removed!");
showMainMenu();
break;
case ServerInfoMessageType::USER_CREATED :
controller->incrementServerResponseNo();
getModel()->setUserId(msg.getExtraInfo());
getModel()->addNotification("User account created!");
showMainMenu();
break;
case ServerInfoMessageType::CATEGORY_JOINED : {
controller->incrementServerResponseNo();
getModel()->addJoinedCategory(msg.getExtraInfo(), msg.getInfo());
getModel()->addNotification("Your request to join category waits for owner confirmation.");
showMainMenu();
break;
}
case ServerInfoMessageType::CATEGORY_LEFT :
controller->incrementServerResponseNo();
getModel()->removeCategoryAndData(msg.getExtraInfo());
getModel()->addNotification("You have successfully left the category!");
showMainMenu();
break;
case ServerInfoMessageType::CATEGORY_ACTIVATED :
controller->incrementServerResponseNo();
getModel()->setCategoryActive(msg.getExtraInfo(), true);
getModel()->addNotification("Category is active again!");
showMainMenu();
break;
case ServerInfoMessageType::CATEGORY_DEACTIVATED :
controller->incrementServerResponseNo();
getModel()->setCategoryActive(msg.getExtraInfo(), false);
getModel()->addNotification("Category deactivated!");
showMainMenu();
break;
case ServerInfoMessageType::NEW_CATEGORY_MEMBER :
getModel()->addPendingUser(msg.getExtraInfo(), msg.getSenderID(), msg.getInfo());
getModel()->addNotification("User pending in one of your categories.");
break;
case ServerInfoMessageType::MEMBER_CONFIRMED: {
getModel()->confirmCategory(msg.getExtraInfo());
stringstream ss;
ss << "Category " << getModel()->getCategoryName(msg.getExtraInfo()) <<
" : owner confirmed your request to join!";
getModel()->addNotification(ss.str());
break;
}
case ServerInfoMessageType::MEMBER_REJECTED: {
stringstream ss2;
ss2 << "Category " << getModel()->getCategoryName(msg.getExtraInfo()) <<
" : owner rejected your request to join!";
getModel()->removeCategoryAndData(msg.getExtraInfo());
getModel()->addNotification(ss2.str());
break;
}
case ServerInfoMessageType::USER_DELETED:
getModel()->removeUserAccount();
getModel()->addNotification("Your account has been removed successfully.");
showMainMenu();
case ServerInfoMessageType::FAIL :
controller->incrementServerResponseNo();
getModel()->addNotification(msg.getInfo());
showMainMenu();
break;
}
}
