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