message.setLocation(GenericROSTranslationTools.convertPoint3D(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setRobotSide(footstep.getRobotSide());
private static Message customConvertToRosMessage(FootstepDataMessage footstep) throws Exception { Class<? extends Packet> ihmcMessageClass = FootstepDataMessage.class; String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcMessageClass.getSimpleName()); RosMessagePacket rosAnnotation = ihmcMessageClass.getAnnotation(RosMessagePacket.class); FootstepDataRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage); message.setUniqueId(footstep.getUniqueId()); message.setLocation(GenericROSTranslationTools.convertTuple3d(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setOrigin((byte) footstep.getOrigin().ordinal()); message.setRobotSide((byte) footstep.getRobotSide().ordinal()); message.setSwingHeight(footstep.getSwingHeight()); message.setTrajectoryType((byte) footstep.getTrajectoryType().ordinal()); List<Point2dRosMessage> predictedContatcPointsRos = new ArrayList<>(); if (footstep.predictedContactPoints != null) { for (Point2d predictedContactPoint : footstep.predictedContactPoints) { predictedContatcPointsRos.add(GenericROSTranslationTools.convertPoint2d(predictedContactPoint)); } } message.setPredictedContactPoints(predictedContatcPointsRos); return message; }
message.setLocation(GenericROSTranslationTools.convertTuple3d(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setOrigin((byte) footstep.getOrigin().ordinal());