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;
}
示例#2
0
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));
}
示例#3
0
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;
}