ROSJAVA Delegate Pattern

ETPROSHandler.java

package pt.edu.academiafa.rosjava.example;

import org.ros.internal.message.Message;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;
import std_msgs.String;

/**
 *
 * @author posilva
 */
public class ETPROSHandler implements ROSNodeDelegate {

    Publisher<std_msgs.String> publisherChatter;

    public Publisher<String> getPublisherChatter() {
        return publisherChatter;
    }

    public ETPROSHandler() {
        this.publisherChatter = null;
    }

    /**
     * recebe as mensagens que o nó com subscribers receber
     *
     * @param message
     */
    public synchronized void subscribedMessage(Message message) {
        if (message instanceof std_msgs.String) {
            System.out.println("Received a 'std_msgs.String' Message: " + ((std_msgs.String) message).getData());
        }
    }

    /**
     * Cria os publishers e expoe por getter para uso através do handler
     *
     * @param connectedNode
     */
    public void setupPublishers(ConnectedNode connectedNode) {
        publisherChatter = connectedNode.newPublisher("chatter", std_msgs.String._TYPE);
    }

    /**
     * Permite instrumentar uma tarefa no nó
     */
    public void loop() {

    }

    /**
     * Permite controlar a taxa do loop
     *
     * @return
     */
    public long getLoopRate() {
        return 1000;//1 segundo
    }
}

ROSJavaExample.java

// ==========================================================================
// Copyright (C) 2013 Força Aérea Portuguesa - Academia                     =
// Centro de Investigação da Academiafa da Força Aérea                      =
// Granja do Marques, Academia, Pero-Pinheiro                               =
// ==========================================================================
// Project: ROSJavaIntegrationExample
// Date:    Dec 7, 2013
// Author:  posilva
// -------------------------------------------------------------------------=
// Change Log:                                                         
// -------------------------------------------------------------------------=
// Date       / Author              /Description                          
// ==========================================================================
//
// Description: ROSJavaExample.java
// ==========================================================================
// $id$
// ==========================================================================
package pt.edu.academiafa.rosjava.example;

import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;

import org.ros.RosCore;

import pt.edu.academiafa.rosjava.ROSManager;

/**
 * Esta classe pretende demonstrar como usar o ROS apenas com Java
 *
 */
public class ROSJavaExample {

    /**
     *
     */
    public ROSJavaExample() {
    }

    /**
     * @param args
     */
    public static void main(String[] args) {
        // !- start roscore - default localhost 11311
        RosCore core = rosCoreLaunch();
        if (core != null) {
            try {
                ROSManager manager = new ROSManager("localhost", 11311);
                ETPROSHandler rosHandler = new ETPROSHandler();
                
                SimplePublisherNode pubNode = new SimplePublisherNode("talker", rosHandler);
                SimpleSubscriberNode subNode = new SimpleSubscriberNode("listener", rosHandler);
                
                manager.startNode(pubNode);
                manager.startNode(subNode);
                
                
                Thread.sleep(2000);
                std_msgs.String str = rosHandler.getPublisherChatter().newMessage();
                str.setData("Hello world! ");
                
                rosHandler.getPublisherChatter().publish(str);
                
                try {
                    System.in.read();
                } catch (IOException e) {
                    
                } finally {
                    
                    manager.shutdown();
                    core.shutdown();
                    System.exit(0);
                }
                
                /*
                SimplePublisherNode pubNode = new SimplePublisherNode("talker", null);
                SimpleSubscriberNode subNode = new SimpleSubscriberNode("listener", null);
                manager.startNode(pubNode);
                manager.startNode(subNode);
                try {
                System.in.read();
                } catch (IOException e) {
                
                } finally {
                
                manager.shutdown();
                core.shutdown();
                System.exit(0);
                }
                */
            } catch (InterruptedException ex) {
                Logger.getLogger(ROSJavaExample.class.getName()).log(Level.SEVERE, null, ex);

            }

            /*
             SimplePublisherNode pubNode = new SimplePublisherNode("talker", null);
             SimpleSubscriberNode subNode = new SimpleSubscriberNode("listener", null);
             manager.startNode(pubNode);
             manager.startNode(subNode);
             try {
             System.in.read();
             } catch (IOException e) {

             } finally {

             manager.shutdown();
             core.shutdown();
             System.exit(0);
             }
             */
        }

    }

    /**
     * Rotina para lançamento do ROSCORE (lançado numa thread)
     *
     */
    private static RosCore rosCoreLaunch() {
        RosCore core = RosCore.newPublic("localhost", 11311);
        core.start();

        try {
            core.awaitStart();

            return core;
        } catch (InterruptedException e) {
            e.printStackTrace();
            return null;
        }

    }

}

ROSNodeDelegate.java

package pt.edu.academiafa.rosjava.example;

import org.ros.node.ConnectedNode;

/**
 *
 * @author posilva
 */
interface ROSNodeDelegate {
    /**
     * 
     * @param message 
     */
    void subscribedMessage(org.ros.internal.message.Message message);
    /**
     * 
     * @param connectedNode 
     */
    public void setupPublishers(ConnectedNode connectedNode);

    public void loop();

    public long getLoopRate();
}

SimplePublisherNode.java

package pt.edu.academiafa.rosjava.example;

import org.ros.concurrent.CancellableLoop;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;
import pt.edu.academiafa.rosjava.AbstractROSNode;

/**
 *
 * @author posilva
 */
class SimplePublisherNode extends AbstractROSNode {

    private final ROSNodeDelegate rosNodeDelegate;

    public SimplePublisherNode(String name) {
        super(name);
        this.rosNodeDelegate = null;
    }

    /**
     * @param name
     * @param rosNodeDelegate
     */
    public SimplePublisherNode(String name, ROSNodeDelegate rosNodeDelegate) {
        super(name);
        this.rosNodeDelegate = rosNodeDelegate;

    }

    /**
     * Exemplo retirado de:
     * http://docs.rosjava.googlecode.com/hg/rosjava_core/html/getting_started.html#publishers-and-subscribers
     *
     * @param connectedNode
     */
    @Override
    public void onStart(final ConnectedNode connectedNode) {
       
        if (rosNodeDelegate != null) {
            rosNodeDelegate.setupPublishers(connectedNode);
        }
        connectedNode.executeCancellableLoop(new CancellableLoop() {
            private int sequenceNumber;

            @Override
            protected void setup() {
                sequenceNumber = 0;
            }

            @Override
            protected void loop() throws InterruptedException {
                if (rosNodeDelegate != null) {
                    rosNodeDelegate.loop();
                    Thread.sleep(rosNodeDelegate.getLoopRate());
                }
            }
        });
    }
}

SimpleSubscriberNode.java

package pt.edu.academiafa.rosjava.example;

import org.apache.commons.logging.Log;
import org.ros.message.MessageListener;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Subscriber;

import pt.edu.academiafa.rosjava.AbstractROSNode;

public class SimpleSubscriberNode extends AbstractROSNode {

    private final ROSNodeDelegate rosNodeDelegate;

    public SimpleSubscriberNode(String name) {
        super(name);
        rosNodeDelegate = null;
    }

    /**
     * @param name
     * @param rosNodeDelegate
     */
    public SimpleSubscriberNode(String name, ROSNodeDelegate rosNodeDelegate) {
        super(name);
        this.rosNodeDelegate = rosNodeDelegate;

    }

    /**
     * Exemplo retirado de:
     * http://docs.rosjava.googlecode.com/hg/rosjava_core/html
     * /getting_started.html
     */
    public void onStart(ConnectedNode connectedNode) {
        final Log log = connectedNode.getLog();
        Subscriber<std_msgs.String> subscriber = connectedNode.newSubscriber(
                "chatter", std_msgs.String._TYPE);
        subscriber.addMessageListener(new MessageListener<std_msgs.String>() {

            public void onNewMessage(std_msgs.String message) {
                if (rosNodeDelegate != null) {
                    rosNodeDelegate.subscribedMessage(message);
                }
            }
        });
    }
}