bundles/addons/org.eclipse.apogy.addons.ros.ui/src-gen-custom/org/eclipse/apogy/addons/ros/ui/impl/TFDisplay3DToolCustomImpl.java

/*******************************************************************************
 * 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.ui.impl;

import org.eclipse.apogy.addons.ros.ApogyAddonsROSFacade;
import org.eclipse.apogy.addons.ros.ApogyAddonsROSFactory;
import org.eclipse.apogy.addons.ros.ApogyAddonsROSPackage;
import org.eclipse.apogy.addons.ros.ROSNode;
import org.eclipse.apogy.addons.ros.ui.TFDisplay3DTool;
import org.eclipse.apogy.addons.ros.utilities.GeometryUtils;
import org.eclipse.apogy.common.math.Matrix4x4;
import org.eclipse.apogy.common.topology.ApogyCommonTopologyFactory;
import org.eclipse.apogy.common.topology.GroupNode;
import org.eclipse.apogy.common.topology.Node;
import org.eclipse.apogy.common.topology.TransformNode;
import org.eclipse.apogy.common.topology.ui.NodePresentation;
import org.eclipse.emf.common.notify.Notification;
import org.eclipse.emf.common.notify.impl.AdapterImpl;
import org.ros.message.MessageListener;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import geometry_msgs.Transform;

public class TFDisplay3DToolCustomImpl extends TFDisplay3DToolImpl {

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

	private TransformListener transformListener;
	private TransformNode transformNode;

	@Override
	public void setActive(boolean newActive) {

		if (!isActive() && newActive) {
			start();
		} else if (isActive() && !newActive) {
			stop();
		}
		super.setActive(newActive);
	}

	@Override
	public void setVisible(boolean newVisible) {
		super.setVisible(newVisible);

		NodePresentation nodePresentation = org.eclipse.apogy.common.topology.ui.Activator
				.getTopologyPresentationRegistry().getPresentationNode(this.transformNode);
		if (nodePresentation != null) {
			// Toggle visibility flag.
			nodePresentation.setVisible(isVisible() && isActive());
		}
	}

	@Override
	public void setRootNode(Node newRootNode) {
		if (getRootNode() instanceof GroupNode) {
			GroupNode previousGroupNode = (GroupNode) getRootNode();
			previousGroupNode.getChildren().remove(getTransformNode());
		}

		super.setRootNode(newRootNode);

		if (newRootNode instanceof GroupNode) {
			GroupNode newGroupNode = (GroupNode) newRootNode;
			newGroupNode.getChildren().add(getTransformNode());
		}
	}

	@Override
	public void dispose() {
		// Detach the transform node from root.
		if (this.transformNode != null && this.transformNode.getParent() instanceof GroupNode) {
			((GroupNode) this.transformNode.getParent()).getChildren().remove(this.transformNode);
			this.transformNode.setParent(null);
			this.transformNode = null;
		}

		super.dispose();
	}

	@Override
	public boolean start() {
		// Initialize the ROS node.
		ROSNode node = ApogyAddonsROSFactory.eINSTANCE.createROSNode();
		node.setNodeName("/TFDisplayClient" + ApogyAddonsROSFacade.INSTANCE.getNodeNamePrefix());
		node.setEnableAutoRestartOnConnectionLost(false);

		Logger.info("RosNode Initializing");
		try {
			node.initialize();
		} catch (Exception e) {
			throw new RuntimeException("Unable to initialize the TFDisplay3DTool ROS Client");
		}

		setNode(node);
		node.register(this, false);

		node.eAdapters().add(new AdapterImpl() {
			@Override
			public void notifyChanged(Notification msg) {
				if (msg.getFeatureID(ROSNode.class) == ApogyAddonsROSPackage.ROS_NODE__CONNECTED) {
					boolean connected = msg.getNewBooleanValue();
					if (connected) {
						Logger.info("Connected to the topic.");

						// TODO : TRANSACTION
						setConnected(true);
					}
				}
			}
		});

		node.start();
		return true;
	}

	@Override
	public boolean stop() {
		if (getNode() != null) {
			try {
				// Shuts down the ROS Node.
				getNode().shutdown();

				// Clears the ROS node.
				setNode(null);

			} catch (Throwable t) {
				Logger.error("stop(): failed!", t);

				// TODO : TRANSACTION
				setConnected(false);

				return false;
			}
		}

		// TODO : TRANSACTION
		setConnected(false);

		return true;
	}

	@Override
	public void rosInit() {
		Logger.info("rosInit() on topic <" + getTopicName() + ">.");

		// Topics Initialization.
		this.topicLauncher.createListener(getTopicName(), geometry_msgs.Transform._TYPE, new TransformListener(this));

		if (!this.topicLauncher.isRunning())
			this.topicLauncher.launch();
	}

	protected TransformNode getTransformNode() {
		if (this.transformNode == null) {
			this.transformNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
			this.transformNode.setNodeId("TFDisplay3DTool");
			this.transformNode.setDescription("TF : <" + getTopicName() + ">");

			NodePresentation nodePresentation = org.eclipse.apogy.common.topology.ui.Activator
					.getTopologyPresentationRegistry().getPresentationNode(this.transformNode);
			if (nodePresentation != null) {
				// Toggle visibility flag.
				nodePresentation.setVisible(isVisible() && isActive());
			}
		}

		return this.transformNode;
	}

	protected TransformListener getTransformListener() {
		if (this.transformListener == null) {
			this.transformListener = new TransformListener(this);
		}
		return this.transformListener;
	}

	private class TransformListener implements MessageListener<geometry_msgs.Transform> {
		private final TFDisplay3DTool tfDisplay3DTool;

		public TransformListener(TFDisplay3DTool newTFDisplay3DTool) {
			this.tfDisplay3DTool = newTFDisplay3DTool;
		}

		@Override
		public void onNewMessage(Transform msg) {
			if (getTransformNode() != null && this.tfDisplay3DTool.isActive()) {
				Matrix4x4 matrix = GeometryUtils.rosTransformToMatrix(msg);
				getTransformNode().setTransformation(matrix.asMatrix4d());
			}
		}
	}
} // TFDisplay3DToolImpl