Java tutorial
/* * Copyright (C) 2011 Google Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); you may not * use this file except in compliance with the License. You may obtain a copy of * the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations under * the License. */ package org.ros.internal.node; import com.google.common.annotations.VisibleForTesting; import org.apache.commons.logging.Log; import org.ros.Parameters; import org.ros.concurrent.CancellableLoop; import org.ros.concurrent.ListenerGroup; import org.ros.concurrent.SignalRunnable; import org.ros.exception.RemoteException; import org.ros.exception.ServiceNotFoundException; import org.ros.internal.message.service.ServiceDescription; import org.ros.internal.message.topic.TopicDescription; import org.ros.internal.node.client.MasterClient; import org.ros.internal.node.client.Registrar; import org.ros.internal.node.parameter.DefaultParameterTree; import org.ros.internal.node.parameter.ParameterManager; import org.ros.internal.node.response.Response; import org.ros.internal.node.response.StatusCode; import org.ros.internal.node.server.NodeIdentifier; import org.ros.internal.node.server.SlaveServer; import org.ros.internal.node.service.ServiceDeclaration; import org.ros.internal.node.service.ServiceFactory; import org.ros.internal.node.service.ServiceIdentifier; import org.ros.internal.node.service.ServiceManager; import org.ros.internal.node.topic.PublisherFactory; import org.ros.internal.node.topic.SubscriberFactory; import org.ros.internal.node.topic.TopicDeclaration; import org.ros.internal.node.topic.TopicParticipantManager; import org.ros.internal.node.xmlrpc.XmlRpcTimeoutException; import org.ros.message.MessageDeserializer; import org.ros.message.MessageFactory; import org.ros.message.MessageSerializationFactory; import org.ros.message.MessageSerializer; import org.ros.message.Time; import org.ros.namespace.GraphName; import org.ros.namespace.NameResolver; import org.ros.namespace.NodeNameResolver; import org.ros.node.ConnectedNode; import org.ros.node.DefaultNodeFactory; import org.ros.node.Node; import org.ros.node.NodeConfiguration; import org.ros.node.NodeListener; import org.ros.node.parameter.ParameterTree; import org.ros.node.service.ServiceClient; import org.ros.node.service.ServiceResponseBuilder; import org.ros.node.service.ServiceServer; import org.ros.node.topic.DefaultPublisherListener; import org.ros.node.topic.DefaultSubscriberListener; import org.ros.node.topic.Publisher; import org.ros.node.topic.Subscriber; import org.ros.time.ClockTopicTimeProvider; import org.ros.time.TimeProvider; import java.net.InetSocketAddress; import java.net.URI; import java.util.Collection; import java.util.concurrent.CountDownLatch; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; /** * The default implementation of a {@link Node}. * * @author ethan.rublee@gmail.com (Ethan Rublee) * @author kwc@willowgarage.com (Ken Conley) * @author damonkohler@google.com (Damon Kohler) */ public class DefaultNode implements ConnectedNode { private static final boolean DEBUG = false; // TODO(damonkohler): Move to NodeConfiguration. /** * The maximum delay before shutdown will begin even if all * {@link NodeListener}s have not yet returned from their * {@link NodeListener#onShutdown(Node)} callback. */ private static final int MAX_SHUTDOWN_DELAY_DURATION = 5; private static final TimeUnit MAX_SHUTDOWN_DELAY_UNITS = TimeUnit.SECONDS; private final NodeConfiguration nodeConfiguration; private final ListenerGroup<NodeListener> nodeListeners; private final ScheduledExecutorService scheduledExecutorService; private final URI masterUri; private final MasterClient masterClient; private final TopicParticipantManager topicParticipantManager; private final ServiceManager serviceManager; private final ParameterManager parameterManager; private final GraphName nodeName; private final NodeNameResolver resolver; private final SlaveServer slaveServer; private final ParameterTree parameterTree; private final PublisherFactory publisherFactory; private final SubscriberFactory subscriberFactory; private final ServiceFactory serviceFactory; private final Registrar registrar; private RosoutLogger log; private TimeProvider timeProvider; /** * {@link DefaultNode}s should only be constructed using the * {@link DefaultNodeFactory}. * * @param nodeConfiguration * the {@link NodeConfiguration} for this {@link Node} * @param nodeListeners * a {@link Collection} of {@link NodeListener}s that will be added * to this {@link Node} before it starts */ public DefaultNode(NodeConfiguration nodeConfiguration, Collection<NodeListener> nodeListeners, ScheduledExecutorService scheduledExecutorService) { this.nodeConfiguration = NodeConfiguration.copyOf(nodeConfiguration); this.nodeListeners = new ListenerGroup<NodeListener>(scheduledExecutorService); this.nodeListeners.addAll(nodeListeners); this.scheduledExecutorService = scheduledExecutorService; masterUri = nodeConfiguration.getMasterUri(); masterClient = new MasterClient(masterUri); topicParticipantManager = new TopicParticipantManager(); serviceManager = new ServiceManager(); parameterManager = new ParameterManager(scheduledExecutorService); GraphName basename = nodeConfiguration.getNodeName(); NameResolver parentResolver = nodeConfiguration.getParentResolver(); nodeName = parentResolver.getNamespace().join(basename); resolver = new NodeNameResolver(nodeName, parentResolver); slaveServer = new SlaveServer(nodeName, nodeConfiguration.getTcpRosBindAddress(), nodeConfiguration.getTcpRosAdvertiseAddress(), nodeConfiguration.getXmlRpcBindAddress(), nodeConfiguration.getXmlRpcAdvertiseAddress(), masterClient, topicParticipantManager, serviceManager, parameterManager, scheduledExecutorService); slaveServer.start(); NodeIdentifier nodeIdentifier = slaveServer.toNodeIdentifier(); parameterTree = DefaultParameterTree.newFromNodeIdentifier(nodeIdentifier, masterClient.getRemoteUri(), resolver, parameterManager); publisherFactory = new PublisherFactory(nodeIdentifier, topicParticipantManager, nodeConfiguration.getTopicMessageFactory(), scheduledExecutorService); subscriberFactory = new SubscriberFactory(nodeIdentifier, topicParticipantManager, scheduledExecutorService); serviceFactory = new ServiceFactory(nodeName, slaveServer, serviceManager, scheduledExecutorService); registrar = new Registrar(masterClient, scheduledExecutorService); topicParticipantManager.setListener(registrar); serviceManager.setListener(registrar); scheduledExecutorService.execute(new Runnable() { @Override public void run() { start(); } }); } private void start() { // The Registrar must be started first so that master registration is // possible during startup. registrar.start(slaveServer.toNodeIdentifier()); // During startup, we wait for 1) the RosoutLogger and 2) the TimeProvider. final CountDownLatch latch = new CountDownLatch(2); log = new RosoutLogger(this); log.getPublisher().addListener(new DefaultPublisherListener<rosgraph_msgs.Log>() { @Override public void onMasterRegistrationSuccess(Publisher<rosgraph_msgs.Log> registrant) { latch.countDown(); } }); boolean useSimTime = false; try { useSimTime = parameterTree.has(Parameters.USE_SIM_TIME) && parameterTree.getBoolean(Parameters.USE_SIM_TIME); } catch (Exception e) { signalOnError(e); } if (useSimTime) { ClockTopicTimeProvider clockTopicTimeProvider = new ClockTopicTimeProvider(this); clockTopicTimeProvider.getSubscriber() .addSubscriberListener(new DefaultSubscriberListener<rosgraph_msgs.Clock>() { @Override public void onMasterRegistrationSuccess(Subscriber<rosgraph_msgs.Clock> subscriber) { latch.countDown(); } }); timeProvider = clockTopicTimeProvider; } else { timeProvider = nodeConfiguration.getTimeProvider(); latch.countDown(); } try { latch.await(); } catch (InterruptedException e) { signalOnError(e); shutdown(); return; } signalOnStart(); } @VisibleForTesting Registrar getRegistrar() { return registrar; } private <T> org.ros.message.MessageSerializer<T> newMessageSerializer(String messageType) { return nodeConfiguration.getMessageSerializationFactory().newMessageSerializer(messageType); } @SuppressWarnings("unchecked") private <T> MessageDeserializer<T> newMessageDeserializer(String messageType) { return (MessageDeserializer<T>) nodeConfiguration.getMessageSerializationFactory() .newMessageDeserializer(messageType); } @SuppressWarnings("unchecked") private <T> MessageSerializer<T> newServiceResponseSerializer(String serviceType) { return (MessageSerializer<T>) nodeConfiguration.getMessageSerializationFactory() .newServiceResponseSerializer(serviceType); } @SuppressWarnings("unchecked") private <T> MessageDeserializer<T> newServiceResponseDeserializer(String serviceType) { return (MessageDeserializer<T>) nodeConfiguration.getMessageSerializationFactory() .newServiceResponseDeserializer(serviceType); } @SuppressWarnings("unchecked") private <T> MessageSerializer<T> newServiceRequestSerializer(String serviceType) { return (MessageSerializer<T>) nodeConfiguration.getMessageSerializationFactory() .newServiceRequestSerializer(serviceType); } @SuppressWarnings("unchecked") private <T> MessageDeserializer<T> newServiceRequestDeserializer(String serviceType) { return (MessageDeserializer<T>) nodeConfiguration.getMessageSerializationFactory() .newServiceRequestDeserializer(serviceType); } @Override public <T> Publisher<T> newPublisher(GraphName topicName, String messageType) { GraphName resolvedTopicName = resolveName(topicName); TopicDescription topicDescription = nodeConfiguration.getTopicDescriptionFactory().newFromType(messageType); TopicDeclaration topicDeclaration = TopicDeclaration.newFromTopicName(resolvedTopicName, topicDescription); org.ros.message.MessageSerializer<T> serializer = newMessageSerializer(messageType); return publisherFactory.newOrExisting(topicDeclaration, serializer); } @Override public <T> Publisher<T> newPublisher(String topicName, String messageType) { return newPublisher(GraphName.of(topicName), messageType); } @Override public <T> Subscriber<T> newSubscriber(GraphName topicName, String messageType) { GraphName resolvedTopicName = resolveName(topicName); TopicDescription topicDescription = nodeConfiguration.getTopicDescriptionFactory().newFromType(messageType); TopicDeclaration topicDeclaration = TopicDeclaration.newFromTopicName(resolvedTopicName, topicDescription); MessageDeserializer<T> deserializer = newMessageDeserializer(messageType); Subscriber<T> subscriber = subscriberFactory.newOrExisting(topicDeclaration, deserializer); return subscriber; } @Override public <T> Subscriber<T> newSubscriber(String topicName, String messageType) { return newSubscriber(GraphName.of(topicName), messageType); } @Override public <T, S> ServiceServer<T, S> newServiceServer(GraphName serviceName, String serviceType, ServiceResponseBuilder<T, S> responseBuilder) { GraphName resolvedServiceName = resolveName(serviceName); // TODO(damonkohler): It's rather non-obvious that the URI will be // created later on the fly. ServiceIdentifier identifier = new ServiceIdentifier(resolvedServiceName, null); ServiceDescription serviceDescription = nodeConfiguration.getServiceDescriptionFactory() .newFromType(serviceType); ServiceDeclaration definition = new ServiceDeclaration(identifier, serviceDescription); MessageDeserializer<T> requestDeserializer = newServiceRequestDeserializer(serviceType); MessageSerializer<S> responseSerializer = newServiceResponseSerializer(serviceType); return serviceFactory.newServer(definition, responseBuilder, requestDeserializer, responseSerializer, nodeConfiguration.getServiceResponseMessageFactory()); } @Override public <T, S> ServiceServer<T, S> newServiceServer(String serviceName, String serviceType, ServiceResponseBuilder<T, S> responseBuilder) { return newServiceServer(GraphName.of(serviceName), serviceType, responseBuilder); } @SuppressWarnings("unchecked") @Override public <T, S> ServiceServer<T, S> getServiceServer(GraphName serviceName) { return (ServiceServer<T, S>) serviceManager.getServer(serviceName); } @Override public <T, S> ServiceServer<T, S> getServiceServer(String serviceName) { return getServiceServer(GraphName.of(serviceName)); } @Override public URI lookupServiceUri(GraphName serviceName) { Response<URI> response = masterClient.lookupService(slaveServer.toNodeIdentifier().getName(), resolveName(serviceName).toString()); if (response.getStatusCode() == StatusCode.SUCCESS) { return response.getResult(); } else { return null; } } @Override public URI lookupServiceUri(String serviceName) { return lookupServiceUri(GraphName.of(serviceName)); } @Override public <T, S> ServiceClient<T, S> newServiceClient(GraphName serviceName, String serviceType) throws ServiceNotFoundException { GraphName resolvedServiceName = resolveName(serviceName); URI uri = lookupServiceUri(resolvedServiceName); if (uri == null) { throw new ServiceNotFoundException( "No such service " + resolvedServiceName + " of type " + serviceType); } ServiceDescription serviceDescription = nodeConfiguration.getServiceDescriptionFactory() .newFromType(serviceType); ServiceIdentifier serviceIdentifier = new ServiceIdentifier(resolvedServiceName, uri); ServiceDeclaration definition = new ServiceDeclaration(serviceIdentifier, serviceDescription); MessageSerializer<T> requestSerializer = newServiceRequestSerializer(serviceType); MessageDeserializer<S> responseDeserializer = newServiceResponseDeserializer(serviceType); return serviceFactory.newClient(definition, requestSerializer, responseDeserializer, nodeConfiguration.getServiceRequestMessageFactory()); } @Override public <T, S> ServiceClient<T, S> newServiceClient(String serviceName, String serviceType) throws ServiceNotFoundException { return newServiceClient(GraphName.of(serviceName), serviceType); } @Override public Time getCurrentTime() { return timeProvider.getCurrentTime(); } @Override public GraphName getName() { return nodeName; } @Override public Log getLog() { return log; } @Override public GraphName resolveName(GraphName name) { return resolver.resolve(name); } @Override public GraphName resolveName(String name) { return resolver.resolve(GraphName.of(name)); } @Override public void shutdown() { signalOnShutdown(); // NOTE(damonkohler): We don't want to raise potentially spurious // exceptions during shutdown that would interrupt the process. This is // simply best effort cleanup. for (Publisher<?> publisher : topicParticipantManager.getPublishers()) { publisher.shutdown(); } for (Subscriber<?> subscriber : topicParticipantManager.getSubscribers()) { subscriber.shutdown(); } for (ServiceServer<?, ?> serviceServer : serviceManager.getServers()) { try { Response<Integer> response = masterClient.unregisterService(slaveServer.toNodeIdentifier(), serviceServer); if (DEBUG) { if (response.getResult() == 0) { System.err.println("Failed to unregister service: " + serviceServer.getName()); } } } catch (XmlRpcTimeoutException e) { log.error(e); } catch (RemoteException e) { log.error(e); } } serviceManager.shutdown(); parameterManager.shutdown(); registrar.shutdown(); slaveServer.shutdown(); signalOnShutdownComplete(); nodeListeners.shutdown(); } @Override public URI getMasterUri() { return masterUri; } @Override public NodeNameResolver getResolver() { return resolver; } @Override public ParameterTree getParameterTree() { return parameterTree; } @Override public URI getUri() { return slaveServer.getUri(); } @Override public MessageSerializationFactory getMessageSerializationFactory() { return nodeConfiguration.getMessageSerializationFactory(); } @Override public MessageFactory getTopicMessageFactory() { return nodeConfiguration.getTopicMessageFactory(); } @Override public MessageFactory getServiceRequestMessageFactory() { return nodeConfiguration.getServiceRequestMessageFactory(); } @Override public MessageFactory getServiceResponseMessageFactory() { return nodeConfiguration.getServiceResponseMessageFactory(); } @Override public void addListener(NodeListener listener) { nodeListeners.add(listener); } /** * SignalRunnable all {@link NodeListener}s that the {@link Node} has * experienced an error. * <p> * Each listener is called in a separate thread. */ private void signalOnError(final Throwable throwable) { final Node node = this; nodeListeners.signal(new SignalRunnable<NodeListener>() { @Override public void run(NodeListener listener) { listener.onError(node, throwable); } }); } /** * SignalRunnable all {@link NodeListener}s that the {@link Node} has started. * <p> * Each listener is called in a separate thread. */ private void signalOnStart() { final ConnectedNode connectedNode = this; nodeListeners.signal(new SignalRunnable<NodeListener>() { @Override public void run(NodeListener listener) { listener.onStart(connectedNode); } }); } /** * SignalRunnable all {@link NodeListener}s that the {@link Node} has started * shutting down. * <p> * Each listener is called in a separate thread. */ private void signalOnShutdown() { final Node node = this; try { nodeListeners.signal(new SignalRunnable<NodeListener>() { @Override public void run(NodeListener listener) { listener.onShutdown(node); } }, MAX_SHUTDOWN_DELAY_DURATION, MAX_SHUTDOWN_DELAY_UNITS); } catch (InterruptedException e) { // Ignored since we do not guarantee that all listeners will finish // before // shutdown begins. } } /** * SignalRunnable all {@link NodeListener}s that the {@link Node} has shut * down. * <p> * Each listener is called in a separate thread. */ private void signalOnShutdownComplete() { final Node node = this; nodeListeners.signal(new SignalRunnable<NodeListener>() { @Override public void run(NodeListener listener) { try { listener.onShutdownComplete(node); } catch (Throwable e) { System.out.println(listener); } } }); } @VisibleForTesting InetSocketAddress getAddress() { return slaveServer.getAddress(); } @Override public ScheduledExecutorService getScheduledExecutorService() { return scheduledExecutorService; } @Override public void executeCancellableLoop(final CancellableLoop cancellableLoop) { scheduledExecutorService.execute(cancellableLoop); addListener(new NodeListener() { @Override public void onStart(ConnectedNode connectedNode) { } @Override public void onShutdown(Node node) { cancellableLoop.cancel(); } @Override public void onShutdownComplete(Node node) { } @Override public void onError(Node node, Throwable throwable) { cancellableLoop.cancel(); } }); } }