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