bundles/addons/org.eclipse.apogy.addons.ros.data3d/src-gen-custom/org/eclipse/apogy/addons/ros/data3d/impl/ApogyAddonsROSData3dFacadeCustomImpl.java - gerrit/apogy/apogy - Git at Google

 /*******************************************************************************
  * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
  * which accompanies this distribution, and is available at
  * http://www.eclipse.org/legal/epl-v10.html
  *
  * Contributors:
  *     Pierre Allard,
  *     Regent L'Archeveque,
  *     Sebastien Gemme - initial API and implementation
  * SPDX-License-Identifier: EPL-1.0
  *
  *******************************************************************************/
 package org.eclipse.apogy.addons.ros.data3d.impl;

 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.util.ArrayList;
 import java.util.List;

 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
 import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
 import org.eclipse.apogy.common.geometry.data3d.CartesianOrientationCoordinates;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
 import org.eclipse.apogy.common.geometry.data3d.ColoredCartesianCoordinatesSet;
 import org.eclipse.apogy.common.geometry.data3d.ColoredCartesianPositionCoordinates;
 import org.eclipse.apogy.common.geometry.data3d.Pose;
 import org.jboss.netty.buffer.ChannelBuffer;
 import org.jboss.netty.buffer.ChannelBuffers;
 import org.ros.message.MessageFactory;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;

 import geometry_msgs.Point;
 import geometry_msgs.Quaternion;
 import sensor_msgs.PointCloud2;
 import sensor_msgs.PointField;

 public class ApogyAddonsROSData3dFacadeCustomImpl extends ApogyAddonsROSData3dFacadeImpl {

 	private static final Logger Logger = LoggerFactory.getLogger(ApogyAddonsROSData3dFacadeImpl.class);

 	@Override
 	public CartesianPositionCoordinates convertToCartesianPositionCoordinates(Point rosPoint) {
 		return ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(rosPoint.getX(),
 				rosPoint.getY(), rosPoint.getZ());
 	}

 	@Override
 	public Point convertToROSPoint(CartesianPositionCoordinates point, MessageFactory messageFactory) {
 		Point p = messageFactory.newFromType(Point._TYPE);

 		p.setX(point.getX());
 		p.setY(point.getY());
 		p.setZ(point.getZ());

 		return p;
 	}

 	@Override
 	public Quaternion convertToROSQuaternion(CartesianOrientationCoordinates orientation) {
 		throw new UnsupportedOperationException();
 	}

 	@Override
 	public CartesianOrientationCoordinates convertToCartesianOrientationCoordinates(Quaternion rosQuaternion) {
 		throw new UnsupportedOperationException();
 	}

 	@Override
 	public Pose convertToPose(geometry_msgs.Pose rosPose) {
 		Pose pose = ApogyCommonGeometryData3DFactory.eINSTANCE.createPose();

 		pose.setX(rosPose.getPosition().getX());
 		pose.setY(rosPose.getPosition().getY());
 		pose.setZ(rosPose.getPosition().getZ());

 		CartesianOrientationCoordinates rot = convertToCartesianOrientationCoordinates(rosPose.getOrientation());
 		pose.setXRotation(rot.getXRotation());
 		pose.setYRotation(rot.getYRotation());
 		pose.setZRotation(rot.getZRotation());

 		return pose;
 	}

 	@Override
 	public geometry_msgs.Pose convertToROSPose(Pose pose, MessageFactory messageFactory) {
 		Point position = convertToROSPoint(pose, messageFactory);
 		Quaternion orientation = convertToROSQuaternion(pose);

 		geometry_msgs.Pose rosPose = messageFactory.newFromType(geometry_msgs.Pose._TYPE);
 		rosPose.setPosition(position);
 		rosPose.setOrientation(orientation);

 		return rosPose;
 	}

 	@Override
 	public CartesianCoordinatesSet convertToCartesianCoordinatesSet(PointCloud2 pointCloud2) {
 		// Return null if there is not enough fields (at least x,y,z) in the point
 		// cloud.
 		if (pointCloud2.getFields().size() < 3)
 			return null;

 		Logger.debug("PointCloud2 Details");
 		Logger.info("\t isBigEndian   : " + pointCloud2.getIsBigendian());
 		Logger.debug("\t Points Step   : " + pointCloud2.getPointStep());
 		Logger.debug("\t Row Step      : " + pointCloud2.getRowStep());
 		Logger.debug("\t Height        : " + pointCloud2.getHeight());
 		Logger.debug("\t Width         : " + pointCloud2.getWidth());
 		Logger.debug("\t Data Capacity : " + pointCloud2.getData().capacity());
 		Logger.debug("\t Data Array    : " + pointCloud2.getData().array().length);
 		Logger.debug("\t Is Dense      : " + pointCloud2.getIsDense());

 		List<PointField> pointFields = pointCloud2.getFields();
 		Logger.debug("\t Fields Size   : " + pointFields.size());
 		for (PointField field : pointFields) {
 			Logger.debug("\t\t Field Name     : " + field.getName());
 			Logger.debug("\t\t Field Count    : " + field.getCount());
 			Logger.debug("\t\t Field Datatype : " + field.getDatatype());
 			Logger.debug("\t\t Field Offset   : " + field.getOffset());
 			Logger.debug("");
 		}

 		CartesianCoordinatesSet cartesianCoordinatesSet = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianCoordinatesSet();
 		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

 		int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();

 		int trailingBytes = pointCloud2.getPointStep() - (3 * 4);

 		Logger.debug("\t Count         : " + count);
 		Logger.debug("\t trailingBytes : " + trailingBytes);

 		ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
 		bb.order(pointCloud2.getData().order());
 		bb.position(pointCloud2.getData().arrayOffset());

 		byte[] byteBucket = new byte[trailingBytes * 2];
 		for (int i = 0; i < count; i++) {
 			try {
 				double x = bb.getFloat();
 				double y = bb.getFloat();
 				double z = bb.getFloat();

 				bb.get(byteBucket, 0, trailingBytes);

 				CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
 						.createCartesianPositionCoordinates(x, y, z);
 				points.add(coord);
 			} catch (Exception e) {
 			}
 		}

 		// Adds all the point to the coordinates set.
 		cartesianCoordinatesSet.getPoints().addAll(points);

 		return cartesianCoordinatesSet;
 	}

 	public CartesianCoordinatesSet convertToCartesianCoordinatesSetNew(PointCloud2 pointCloud2) {
 		// TODO : Test this new implementation.

 		// Return null if there is not enough fields (at least x,y,z) in the point
 		// cloud.
 		if (pointCloud2.getFields().size() < 3)
 			return null;

 		Logger.debug("PointCloud2 Details");
 		Logger.debug("\t isBigEndian   : " + pointCloud2.getIsBigendian());
 		Logger.debug("\t Points Step   : " + pointCloud2.getPointStep());
 		Logger.debug("\t Row Step      : " + pointCloud2.getRowStep());
 		Logger.debug("\t Height        : " + pointCloud2.getHeight());
 		Logger.debug("\t Width         : " + pointCloud2.getWidth());
 		Logger.debug("\t Data Capacity : " + pointCloud2.getData().capacity());
 		Logger.debug("\t Data Array    : " + pointCloud2.getData().array().length);
 		Logger.debug("\t Is Dense      : " + pointCloud2.getIsDense());

 		List<PointField> pointFields = pointCloud2.getFields();
 		Logger.debug("\t Fields Size   : " + pointFields.size());
 		for (PointField field : pointFields) {
 			Logger.debug("\t\t Field Name     : " + field.getName());
 			Logger.debug("\t\t Field Count    : " + field.getCount());
 			Logger.debug("\t\t Field Datatype : " + field.getDatatype());
 			Logger.debug("\t\t Field Offset   : " + field.getOffset());
 			Logger.debug("");
 		}

 		CartesianCoordinatesSet cartesianCoordinatesSet = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianCoordinatesSet();
 		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

 		int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();

 		int pointStep = pointCloud2.getPointStep();
 		int xIndex = -1;
 		int yIndex = -1;
 		int zIndex = -1;

 		// Extract the index of x,y and z.
 		pointFields = pointCloud2.getFields();
 		for (PointField field : pointFields) {
 			Logger.info("\t\t Field Name     : " + field.getName());

 			if (field.getName() != null && field.getName().length() > 0) {
 				if (field.getName().trim().compareToIgnoreCase("x") == 0 && xIndex == -1)
 					xIndex = field.getOffset();
 				if (field.getName().trim().compareToIgnoreCase("y") == 0 && yIndex == -1)
 					yIndex = field.getOffset();
 				if (field.getName().trim().compareToIgnoreCase("z") == 0 && zIndex == -1)
 					zIndex = field.getOffset();
 			}
 		}

 		// Wraps received data in a buffer.
 		ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
 		bb.order(pointCloud2.getData().order());
 		bb.position(pointCloud2.getData().arrayOffset());

 		// Reads and parse each point binary content.
 		byte[] pointRawData = new byte[pointCloud2.getPointStep()];
 		for (int i = 0; i < count; i++) {
 			try {
 				// Reads the bytes associated with one point.
 				bb.get(pointRawData, 0, pointStep);

 				// Wraps point data in a buffer.
 				ByteBuffer pointBuffer = ByteBuffer.wrap(pointRawData);
 				pointBuffer.order(pointCloud2.getData().order());
 				pointBuffer.position(0);

 				// Extracts the coordinates from the point.
 				double x = pointBuffer.getFloat(xIndex);
 				double y = pointBuffer.getFloat(yIndex);
 				double z = pointBuffer.getFloat(zIndex);

 				CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
 						.createCartesianPositionCoordinates(x, y, z);
 				points.add(coord);
 			} catch (Exception e) {
 			}
 		}

 		// Adds all the points to the coordinates set.
 		cartesianCoordinatesSet.getPoints().addAll(points);

 		return cartesianCoordinatesSet;
 	}

 	@Override
 	public ColoredCartesianCoordinatesSet rosPointCloudToCartesianCoordinateSet(PointCloud2 pointCloud2) {
 		ColoredCartesianCoordinatesSet coordinates = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createColoredCartesianCoordinatesSet();
 		List<ColoredCartesianPositionCoordinates> points = new ArrayList<ColoredCartesianPositionCoordinates>();

 		int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();
 		ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
 		bb.order(pointCloud2.getData().order());
 		bb.position(pointCloud2.getData().arrayOffset());

 		boolean color = (pointCloud2.getFields().size() > 3);

 		for (int i = 0; i < count; i++) {
 			ColoredCartesianPositionCoordinates coord;
 			if (color) {
 				coord = readRGBCartesianPositionCoordinates(bb);
 			} else {
 				CartesianPositionCoordinates tmp = readCartesianPositionCoordinates(bb);
 				coord = ApogyCommonGeometryData3DFacade.INSTANCE.createColoredCartesianPositionCoordinates(tmp.getX(),
 						tmp.getY(), tmp.getZ(), (short) 255, (short) 255, (short) 255);
 			}
 			points.add(coord);
 		}

 		coordinates.getPoints().addAll(points);

 		return coordinates;
 	}

 	@Override
 	public ColoredCartesianPositionCoordinates readRGBCartesianPositionCoordinates(ByteBuffer byteBuffer) {
 		float x = byteBuffer.getFloat();
 		float y = byteBuffer.getFloat();
 		float z = byteBuffer.getFloat();

 		byteBuffer.getFloat(); // Skip 4 bytes

 		long rgb = byteBuffer.getLong();
 		short red = (short) (rgb >> 16 & 0x0000ff);
 		short green = (short) (rgb >> 8 & 0x0000ff);
 		short blue = (short) (rgb >> 0 & 0x0000ff);

 		byteBuffer.getLong(); // Skip 8 bytes

 		return ApogyCommonGeometryData3DFacade.INSTANCE.createColoredCartesianPositionCoordinates(x, y, z, red, green,
 				blue);
 	}

 	@Override
 	public CartesianPositionCoordinates readCartesianPositionCoordinates(ByteBuffer byteBuffer) {
 		CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
 				.createCartesianPositionCoordinates(byteBuffer.getFloat(), byteBuffer.getFloat(),
 						byteBuffer.getFloat());
 		byteBuffer.getFloat();

 		return coord;
 	}

 	@Override
 	public PointCloud2 cartesianCoordinateSetToRosPointCloud(CartesianCoordinatesSet map,
 			MessageFactory messageFactory) {
 		sensor_msgs.PointCloud2 pc = messageFactory.newFromType(sensor_msgs.PointCloud2._TYPE);

 		sensor_msgs.PointField pfx = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
 		pfx.setName("x");
 		pfx.setDatatype(sensor_msgs.PointField.FLOAT32);
 		pfx.setCount(1);

 		sensor_msgs.PointField pfy = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
 		pfy.setName("y");
 		pfy.setDatatype(sensor_msgs.PointField.FLOAT32);
 		pfy.setOffset(4);
 		pfy.setCount(1);

 		sensor_msgs.PointField pfz = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
 		pfz.setName("z");
 		pfz.setDatatype(sensor_msgs.PointField.FLOAT32);
 		pfz.setOffset(8);
 		pfz.setCount(1);

 		pc.getFields().add(pfx);
 		pc.getFields().add(pfy);
 		pc.getFields().add(pfz);

 		pc.setHeight(1);
 		pc.setWidth(map.getPoints().size());
 		pc.setIsBigendian(false);
 		pc.setIsDense(true);
 		pc.setPointStep(16);
 		pc.setRowStep(map.getPoints().size() * 16);

 		ChannelBuffer data = ChannelBuffers.buffer(ByteOrder.LITTLE_ENDIAN, map.getPoints().size() * 16);

 		for (CartesianPositionCoordinates pt : map.getPoints()) {
 			data.writeFloat((float) pt.getX());
 			data.writeFloat((float) pt.getY());
 			data.writeFloat((float) pt.getZ());
 			data.writeFloat(1.0f);
 		}

 		pc.setData(data);

 		return pc;
 	}

 } // ApogyAddonsROSData3dFacadeImpl
	/*******************************************************************************
	* Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
	* All rights reserved. This program and the accompanying materials
	* are made available under the terms of the Eclipse Public License v1.0
	* which accompanies this distribution, and is available at
	* http://www.eclipse.org/legal/epl-v10.html
	*
	* Contributors:
	* Pierre Allard,
	* Regent L'Archeveque,
	* Sebastien Gemme - initial API and implementation
	* SPDX-License-Identifier: EPL-1.0
	*
	*******************************************************************************/
	package org.eclipse.apogy.addons.ros.data3d.impl;

	import java.nio.ByteBuffer;
	import java.nio.ByteOrder;
	import java.util.ArrayList;
	import java.util.List;

	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
	import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
	import org.eclipse.apogy.common.geometry.data3d.CartesianOrientationCoordinates;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
	import org.eclipse.apogy.common.geometry.data3d.ColoredCartesianCoordinatesSet;
	import org.eclipse.apogy.common.geometry.data3d.ColoredCartesianPositionCoordinates;
	import org.eclipse.apogy.common.geometry.data3d.Pose;
	import org.jboss.netty.buffer.ChannelBuffer;
	import org.jboss.netty.buffer.ChannelBuffers;
	import org.ros.message.MessageFactory;
	import org.slf4j.Logger;
	import org.slf4j.LoggerFactory;

	import geometry_msgs.Point;
	import geometry_msgs.Quaternion;
	import sensor_msgs.PointCloud2;
	import sensor_msgs.PointField;

	public class ApogyAddonsROSData3dFacadeCustomImpl extends ApogyAddonsROSData3dFacadeImpl {

	private static final Logger Logger = LoggerFactory.getLogger(ApogyAddonsROSData3dFacadeImpl.class);

	@Override
	public CartesianPositionCoordinates convertToCartesianPositionCoordinates(Point rosPoint) {
	return ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(rosPoint.getX(),
	rosPoint.getY(), rosPoint.getZ());
	}

	@Override
	public Point convertToROSPoint(CartesianPositionCoordinates point, MessageFactory messageFactory) {
	Point p = messageFactory.newFromType(Point._TYPE);

	p.setX(point.getX());
	p.setY(point.getY());
	p.setZ(point.getZ());

	return p;
	}

	@Override
	public Quaternion convertToROSQuaternion(CartesianOrientationCoordinates orientation) {
	throw new UnsupportedOperationException();
	}

	@Override
	public CartesianOrientationCoordinates convertToCartesianOrientationCoordinates(Quaternion rosQuaternion) {
	throw new UnsupportedOperationException();
	}

	@Override
	public Pose convertToPose(geometry_msgs.Pose rosPose) {
	Pose pose = ApogyCommonGeometryData3DFactory.eINSTANCE.createPose();

	pose.setX(rosPose.getPosition().getX());
	pose.setY(rosPose.getPosition().getY());
	pose.setZ(rosPose.getPosition().getZ());

	CartesianOrientationCoordinates rot = convertToCartesianOrientationCoordinates(rosPose.getOrientation());
	pose.setXRotation(rot.getXRotation());
	pose.setYRotation(rot.getYRotation());
	pose.setZRotation(rot.getZRotation());

	return pose;
	}

	@Override
	public geometry_msgs.Pose convertToROSPose(Pose pose, MessageFactory messageFactory) {
	Point position = convertToROSPoint(pose, messageFactory);
	Quaternion orientation = convertToROSQuaternion(pose);

	geometry_msgs.Pose rosPose = messageFactory.newFromType(geometry_msgs.Pose._TYPE);
	rosPose.setPosition(position);
	rosPose.setOrientation(orientation);

	return rosPose;
	}

	@Override
	public CartesianCoordinatesSet convertToCartesianCoordinatesSet(PointCloud2 pointCloud2) {
	// Return null if there is not enough fields (at least x,y,z) in the point
	// cloud.
	if (pointCloud2.getFields().size() < 3)
	return null;

	Logger.debug("PointCloud2 Details");
	Logger.info("\t isBigEndian : " + pointCloud2.getIsBigendian());
	Logger.debug("\t Points Step : " + pointCloud2.getPointStep());
	Logger.debug("\t Row Step : " + pointCloud2.getRowStep());
	Logger.debug("\t Height : " + pointCloud2.getHeight());
	Logger.debug("\t Width : " + pointCloud2.getWidth());
	Logger.debug("\t Data Capacity : " + pointCloud2.getData().capacity());
	Logger.debug("\t Data Array : " + pointCloud2.getData().array().length);
	Logger.debug("\t Is Dense : " + pointCloud2.getIsDense());

	List<PointField> pointFields = pointCloud2.getFields();
	Logger.debug("\t Fields Size : " + pointFields.size());
	for (PointField field : pointFields) {
	Logger.debug("\t\t Field Name : " + field.getName());
	Logger.debug("\t\t Field Count : " + field.getCount());
	Logger.debug("\t\t Field Datatype : " + field.getDatatype());
	Logger.debug("\t\t Field Offset : " + field.getOffset());
	Logger.debug("");
	}

	CartesianCoordinatesSet cartesianCoordinatesSet = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianCoordinatesSet();
	List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

	int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();

	int trailingBytes = pointCloud2.getPointStep() - (3 * 4);

	Logger.debug("\t Count : " + count);
	Logger.debug("\t trailingBytes : " + trailingBytes);

	ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
	bb.order(pointCloud2.getData().order());
	bb.position(pointCloud2.getData().arrayOffset());

	byte[] byteBucket = new byte[trailingBytes * 2];
	for (int i = 0; i < count; i++) {
	try {
	double x = bb.getFloat();
	double y = bb.getFloat();
	double z = bb.getFloat();

	bb.get(byteBucket, 0, trailingBytes);

	CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
	.createCartesianPositionCoordinates(x, y, z);
	points.add(coord);
	} catch (Exception e) {
	}
	}

	// Adds all the point to the coordinates set.
	cartesianCoordinatesSet.getPoints().addAll(points);

	return cartesianCoordinatesSet;
	}

	public CartesianCoordinatesSet convertToCartesianCoordinatesSetNew(PointCloud2 pointCloud2) {
	// TODO : Test this new implementation.

	// Return null if there is not enough fields (at least x,y,z) in the point
	// cloud.
	if (pointCloud2.getFields().size() < 3)
	return null;

	Logger.debug("PointCloud2 Details");
	Logger.debug("\t isBigEndian : " + pointCloud2.getIsBigendian());
	Logger.debug("\t Points Step : " + pointCloud2.getPointStep());
	Logger.debug("\t Row Step : " + pointCloud2.getRowStep());
	Logger.debug("\t Height : " + pointCloud2.getHeight());
	Logger.debug("\t Width : " + pointCloud2.getWidth());
	Logger.debug("\t Data Capacity : " + pointCloud2.getData().capacity());
	Logger.debug("\t Data Array : " + pointCloud2.getData().array().length);
	Logger.debug("\t Is Dense : " + pointCloud2.getIsDense());

	List<PointField> pointFields = pointCloud2.getFields();
	Logger.debug("\t Fields Size : " + pointFields.size());
	for (PointField field : pointFields) {
	Logger.debug("\t\t Field Name : " + field.getName());
	Logger.debug("\t\t Field Count : " + field.getCount());
	Logger.debug("\t\t Field Datatype : " + field.getDatatype());
	Logger.debug("\t\t Field Offset : " + field.getOffset());
	Logger.debug("");
	}

	CartesianCoordinatesSet cartesianCoordinatesSet = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianCoordinatesSet();
	List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

	int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();

	int pointStep = pointCloud2.getPointStep();
	int xIndex = -1;
	int yIndex = -1;
	int zIndex = -1;

	// Extract the index of x,y and z.
	pointFields = pointCloud2.getFields();
	for (PointField field : pointFields) {
	Logger.info("\t\t Field Name : " + field.getName());

	if (field.getName() != null && field.getName().length() > 0) {
	if (field.getName().trim().compareToIgnoreCase("x") == 0 && xIndex == -1)
	xIndex = field.getOffset();
	if (field.getName().trim().compareToIgnoreCase("y") == 0 && yIndex == -1)
	yIndex = field.getOffset();
	if (field.getName().trim().compareToIgnoreCase("z") == 0 && zIndex == -1)
	zIndex = field.getOffset();
	}
	}

	// Wraps received data in a buffer.
	ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
	bb.order(pointCloud2.getData().order());
	bb.position(pointCloud2.getData().arrayOffset());

	// Reads and parse each point binary content.
	byte[] pointRawData = new byte[pointCloud2.getPointStep()];
	for (int i = 0; i < count; i++) {
	try {
	// Reads the bytes associated with one point.
	bb.get(pointRawData, 0, pointStep);

	// Wraps point data in a buffer.
	ByteBuffer pointBuffer = ByteBuffer.wrap(pointRawData);
	pointBuffer.order(pointCloud2.getData().order());
	pointBuffer.position(0);

	// Extracts the coordinates from the point.
	double x = pointBuffer.getFloat(xIndex);
	double y = pointBuffer.getFloat(yIndex);
	double z = pointBuffer.getFloat(zIndex);

	CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
	.createCartesianPositionCoordinates(x, y, z);
	points.add(coord);
	} catch (Exception e) {
	}
	}

	// Adds all the points to the coordinates set.
	cartesianCoordinatesSet.getPoints().addAll(points);

	return cartesianCoordinatesSet;
	}

	@Override
	public ColoredCartesianCoordinatesSet rosPointCloudToCartesianCoordinateSet(PointCloud2 pointCloud2) {
	ColoredCartesianCoordinatesSet coordinates = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createColoredCartesianCoordinatesSet();
	List<ColoredCartesianPositionCoordinates> points = new ArrayList<ColoredCartesianPositionCoordinates>();

	int count = pointCloud2.getData().capacity() / pointCloud2.getPointStep();
	ByteBuffer bb = ByteBuffer.wrap(pointCloud2.getData().array());
	bb.order(pointCloud2.getData().order());
	bb.position(pointCloud2.getData().arrayOffset());

	boolean color = (pointCloud2.getFields().size() > 3);

	for (int i = 0; i < count; i++) {
	ColoredCartesianPositionCoordinates coord;
	if (color) {
	coord = readRGBCartesianPositionCoordinates(bb);
	} else {
	CartesianPositionCoordinates tmp = readCartesianPositionCoordinates(bb);
	coord = ApogyCommonGeometryData3DFacade.INSTANCE.createColoredCartesianPositionCoordinates(tmp.getX(),
	tmp.getY(), tmp.getZ(), (short) 255, (short) 255, (short) 255);
	}
	points.add(coord);
	}

	coordinates.getPoints().addAll(points);

	return coordinates;
	}

	@Override
	public ColoredCartesianPositionCoordinates readRGBCartesianPositionCoordinates(ByteBuffer byteBuffer) {
	float x = byteBuffer.getFloat();
	float y = byteBuffer.getFloat();
	float z = byteBuffer.getFloat();

	byteBuffer.getFloat(); // Skip 4 bytes

	long rgb = byteBuffer.getLong();
	short red = (short) (rgb >> 16 & 0x0000ff);
	short green = (short) (rgb >> 8 & 0x0000ff);
	short blue = (short) (rgb >> 0 & 0x0000ff);

	byteBuffer.getLong(); // Skip 8 bytes

	return ApogyCommonGeometryData3DFacade.INSTANCE.createColoredCartesianPositionCoordinates(x, y, z, red, green,
	blue);
	}

	@Override
	public CartesianPositionCoordinates readCartesianPositionCoordinates(ByteBuffer byteBuffer) {
	CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFacade.INSTANCE
	.createCartesianPositionCoordinates(byteBuffer.getFloat(), byteBuffer.getFloat(),
	byteBuffer.getFloat());
	byteBuffer.getFloat();

	return coord;
	}

	@Override
	public PointCloud2 cartesianCoordinateSetToRosPointCloud(CartesianCoordinatesSet map,
	MessageFactory messageFactory) {
	sensor_msgs.PointCloud2 pc = messageFactory.newFromType(sensor_msgs.PointCloud2._TYPE);

	sensor_msgs.PointField pfx = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
	pfx.setName("x");
	pfx.setDatatype(sensor_msgs.PointField.FLOAT32);
	pfx.setCount(1);

	sensor_msgs.PointField pfy = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
	pfy.setName("y");
	pfy.setDatatype(sensor_msgs.PointField.FLOAT32);
	pfy.setOffset(4);
	pfy.setCount(1);

	sensor_msgs.PointField pfz = messageFactory.newFromType(sensor_msgs.PointField._TYPE);
	pfz.setName("z");
	pfz.setDatatype(sensor_msgs.PointField.FLOAT32);
	pfz.setOffset(8);
	pfz.setCount(1);

	pc.getFields().add(pfx);
	pc.getFields().add(pfy);
	pc.getFields().add(pfz);

	pc.setHeight(1);
	pc.setWidth(map.getPoints().size());
	pc.setIsBigendian(false);
	pc.setIsDense(true);
	pc.setPointStep(16);
	pc.setRowStep(map.getPoints().size() * 16);

	ChannelBuffer data = ChannelBuffers.buffer(ByteOrder.LITTLE_ENDIAN, map.getPoints().size() * 16);

	for (CartesianPositionCoordinates pt : map.getPoints()) {
	data.writeFloat((float) pt.getX());
	data.writeFloat((float) pt.getY());
	data.writeFloat((float) pt.getZ());
	data.writeFloat(1.0f);
	}

	pc.setData(data);

	return pc;
	}

	} // ApogyAddonsROSData3dFacadeImpl