private static void setVecmathFieldFromRosGeometryMessage(Message rosMessage, Packet<?> ihmcMessage, Method rosGetter, Field ihmcField, Class<?> ihmcMessageFieldType) throws IllegalAccessException, InvocationTargetException, InstantiationException { if(ihmcMessageFieldType.equals(Point2d.class)) { Point2d point2d = convertPoint2DRos((Point2dRosMessage) rosGetter.invoke(rosMessage)); ihmcField.set(ihmcMessage, point2d); } else { if(rosGetter.getReturnType().equals(Quaternion.class)) { Tuple4d newTuple = (Tuple4d) ihmcField.getType().newInstance(); newTuple.set(convertQuaternion((Quaternion) rosGetter.invoke(rosMessage))); ihmcField.set(ihmcMessage, newTuple); } else if(rosGetter.getReturnType().equals(Vector3.class)) { Tuple3d newTuple = (Tuple3d) ihmcField.getType().newInstance(); newTuple.set(convertVector3((Vector3) rosGetter.invoke(rosMessage))); ihmcField.set(ihmcMessage, newTuple); } } }