bundles/addons/org.eclipse.apogy.addons.ros/src-gen-custom/org/eclipse/apogy/addons/ros/impl/ROSServiceCustomImpl.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.impl;

import java.util.concurrent.TimeUnit;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.ReentrantLock;

import org.eclipse.apogy.addons.ros.ROSNode;
import org.eclipse.apogy.addons.ros.ROSServiceState;
import org.eclipse.apogy.addons.ros.utilities.AsynchronousShutdowner;
import org.jboss.netty.handler.timeout.TimeoutException;
import org.ros.exception.RemoteException;
import org.ros.exception.ServiceNotFoundException;
import org.ros.internal.message.Message;
import org.ros.internal.message.RawMessage;
import org.ros.internal.message.field.Field;
import org.ros.node.service.ServiceClient;
import org.ros.node.service.ServiceResponseListener;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class ROSServiceCustomImpl<Request extends Message, Response extends Message>
		extends ROSServiceImpl<Request, Response> {
	private static final Logger Logger = LoggerFactory.getLogger(ROSServiceImpl.class);

	final int MAX_WAIT_TIME = 5000; // milliseconds

	@Override
	@SuppressWarnings("unchecked")
	public void launch(ROSNode node) throws ServiceNotFoundException {
		setNode(node);
		setServiceClient((ServiceClient<Request, Response>) getNode().getConnectedNode()
				.newServiceClient(getServiceName(), getServiceType()));
		setLaunched(true);
		setServiceState(ROSServiceState.READY);
	}

	@Override
	public void stop() {
		try {
			AsynchronousShutdowner.add(getServiceClient());
		} catch (Exception e) {
		}

		setServiceState(ROSServiceState.STOPPED);
		setServiceClient(null);
		setLaunched(false);
	}

	@Override
	@SuppressWarnings("unchecked")
	public Request newRequestMessage() {
		if (isLaunched())
			return getServiceClient().newMessage();
		else if (getNode().getNullRequestHandler() != null)
			return (Request) getNode().getNullRequestHandler().handleNullRequest(this);
		return null;
	}

	@Override
	public Response call(Request request) {
		return call(request, true);
	}

	@Override
	public Response call(Request request, boolean enableLogging) {
		return call(request, enableLogging, this.MAX_WAIT_TIME);
	}

	@Override
	@SuppressWarnings("unchecked")
	public Response call(Request request, boolean enableLogging, int timeout) {
		Response response = null;
		ROSNode node = getNode();

		if (isLaunched()) {
			if (enableLogging)
				logRequest(request);

			BlockingServiceCall<Response> listener = new BlockingServiceCall<Response>(request, timeout);

			try {
				getServiceClient().call(request, listener);
				response = listener.getResponse();
			} catch (Exception e) {
				throw new RuntimeException(e);
			}
		}

		if (response == null && node.getNullResponseHandler() != null) {
			response = (Response) node.getNullResponseHandler().handleNullResponse(this);
		}

		if (enableLogging) {
			if (node.getResponseLogger() != null) {
				node.getResponseLogger().logResponse(this, response);
			}
		}

		return response;
	}

	private void logRequest(Request request) {
		StringBuilder strBuilder = new StringBuilder();

		strBuilder.append("Service ");
		strBuilder.append(this.serviceName);
		strBuilder.append(" called with parameters : ");
		generateParametersString(strBuilder, request);

		Logger.info(strBuilder.toString());
	}

	private void generateParametersString(StringBuilder builder, Message msg) {
		RawMessage rmsg = msg.toRawMessage();

		for (Field f : rmsg.getFields())
			if (!f.isConstant()) {
				builder.append("{ ");
				builder.append(f.getName());
				builder.append(": ");

				if (f.getValue() instanceof Message)
					generateParametersString(builder, (Message) f.getValue());
				else
					builder.append(f.getValue().toString());
				builder.append(" } ");
			}
	}

	private class BlockingServiceCall<T extends Message> implements ServiceResponseListener<T> {
		final boolean USE_MAX_WAIT_TIME_DEFAULT = true;

		private boolean useMaxWaitTime = this.USE_MAX_WAIT_TIME_DEFAULT;
		private boolean done = false;
		private Condition condition = null;
		private ReentrantLock lock;
		private T response;
		private int timeoutMilliseconds = ROSServiceCustomImpl.this.MAX_WAIT_TIME;

		private Request request = null;

		public BlockingServiceCall(Request request) {
			this.lock = new ReentrantLock();
			this.condition = this.lock.newCondition();
			this.request = request;
		}

		public BlockingServiceCall(Request request, int timeoutMilliseconds) {
			this(request);

			this.timeoutMilliseconds = timeoutMilliseconds;

			if (timeoutMilliseconds < 0) {
				this.useMaxWaitTime = false;
			} else {
				this.useMaxWaitTime = true;
			}

		}

		@Override
		public void onSuccess(T response) {
			this.lock.lock();
			try {
				this.response = response;
				this.done = true;
				this.condition.signal();
			} finally {
				this.lock.unlock();
			}
		}

		@Override
		public void onFailure(RemoteException e) {
			Logger.error(e.getMessage(), e);
			this.lock.lock();
			try {
				this.done = true;
				this.condition.signal();
			} finally {
				this.lock.unlock();
			}
		}

		public T getResponse() {
			this.lock.lock();
			try {
				try {
					if (this.useMaxWaitTime) {
						this.done = this.condition.await(this.timeoutMilliseconds, TimeUnit.MILLISECONDS);

						if (!this.done) {
							if (isDisconnectOnTimeout()) {
								ROSServiceCustomImpl.this.node.setConnected(false);
							}

							// Generate an error message.
							StringBuilder strBuilder = new StringBuilder();
							strBuilder.append("Service ");
							strBuilder.append(ROSServiceCustomImpl.this.serviceName);
							strBuilder.append(" called with parameters : ");
							generateParametersString(strBuilder, this.request);
							strBuilder.append(" : Timed out !");
							Logger.error(strBuilder.toString());

							throw new TimeoutException(strBuilder.toString());
						}
					} else {
						this.condition.await();
					}
				} catch (InterruptedException e) {

				}
			} finally {
				this.lock.unlock();
			}
			return this.response;
		}
	}

} // ROSServiceImpl