private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message) throws Exception { WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage(); wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId()); wholeBodyTrajectoryMessage.leftArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage()); wholeBodyTrajectoryMessage.rightArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage()); wholeBodyTrajectoryMessage.leftHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage()); wholeBodyTrajectoryMessage.rightHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage()); wholeBodyTrajectoryMessage.leftFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage()); wholeBodyTrajectoryMessage.rightFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage()); wholeBodyTrajectoryMessage.chestTrajectoryMessage = (ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage()); wholeBodyTrajectoryMessage.pelvisTrajectoryMessage = (PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage()); return wholeBodyTrajectoryMessage; }
private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message) throws Exception { WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage(); wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId()); wholeBodyTrajectoryMessage.leftArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage()); wholeBodyTrajectoryMessage.rightArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage()); wholeBodyTrajectoryMessage.leftHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage()); wholeBodyTrajectoryMessage.rightHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage()); wholeBodyTrajectoryMessage.leftFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage()); wholeBodyTrajectoryMessage.rightFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage()); wholeBodyTrajectoryMessage.chestTrajectoryMessage = (ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage()); wholeBodyTrajectoryMessage.pelvisTrajectoryMessage = (PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage()); return wholeBodyTrajectoryMessage; }
private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message) { WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage(); wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId()); try { wholeBodyTrajectoryMessage.getLeftArmTrajectoryMessage().set((ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage())); wholeBodyTrajectoryMessage.getRightArmTrajectoryMessage().set((ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage())); wholeBodyTrajectoryMessage.getLeftHandTrajectoryMessage().set((HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage())); wholeBodyTrajectoryMessage.getRightHandTrajectoryMessage().set((HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage())); wholeBodyTrajectoryMessage.getLeftFootTrajectoryMessage().set((FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage())); wholeBodyTrajectoryMessage.getRightFootTrajectoryMessage().set((FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage())); wholeBodyTrajectoryMessage.getChestTrajectoryMessage().set((ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage())); wholeBodyTrajectoryMessage.getPelvisTrajectoryMessage().set((PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage())); wholeBodyTrajectoryMessage.getHeadTrajectoryMessage().set((HeadTrajectoryMessage) convertToIHMCMessage(message.getHeadTrajectoryMessage())); } catch (ClassNotFoundException | InvocationTargetException | IllegalAccessException | RosEnumConversionException | NoSuchFieldException | InstantiationException | IllegalArgumentException | NoSuchMethodException | SecurityException e) { e.printStackTrace(); } return wholeBodyTrajectoryMessage; }