rosjava_core 0.1.6 documentation

Getting started

«  Best practices   ::   Contents   ::   Advanced topics  »

Getting started

Creating a new Java package

Please refer to the RosWiki for more information on how to create a new gradle/catkin project and subprojects. You might also wish to read the Gradle Java tutorial for more details about building with gradle itself.

Creating nodes

Typically ROS nodes are synonymous with processes. In rosjava, however, nodes are more like nodelets in that many nodes can run in a single process, the Java VM.

Users, like yourself, do not create Nodes. Instead, programs are defined as implementations of NodeMain which are executed by the aptly named NodeMainExecutor.

Let’s consider the following mostly empty NodeMain implementation:

import org.ros.namespace.GraphName;
import org.ros.node.Node;
import org.ros.node.NodeMain;

public class MyNode implements NodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return new GraphName("my_node");
  }

  @Override
  public void onStart(ConnectedNode node) {
  }

  @Override
  public void onShutdown(Node node) {
  }

  @Override
  public void onShutdownComplete(Node node) {
  }

  @Override
  public void onError(Node node, Throwable throwable) {
  }
}

The NodeMain.getDefaultNodeName method returns the default name of the node. This name will be used unless a node name is specified in the NodeConfiguration (more on that later). GraphNames are used throughout rosjava when refering to nodes, topics, and parameters. Most methods which accept a GraphName will also accept a string for convenience.

The NodeListener.onStart method is the entry point for your program (or node). The ConnectedNode parameter is the factory we use to build things like Publishers and Subscribers.

The NodeListener.onShutdown method is the first exit point for your program. It will be executed as soon as shutdown is started (i.e. before all publishers, subscribers, etc. have been shutdown). The shutdown of all created publishers, subscribers, etc. will be delayed until this method returns or the shutdown timeout expires.

The NodeListener.onShutdownComplete method is the final exit point for your program. It will be executed after all publishers, subscribers, etc. have been shutdown. This is the preferred place to handle clean up since it will not delay shutdown.

The NodeListener.onError method is called when an error occurs in the Node itself. These errors are typically fatal. The NodeListener.onShutdown and NodeListener.onShutdownComplete methods will be called following the call to NodeListener.onError.

Publishers and subscribers

The following class (Talker) is available from the rosjava_tutorial_pubsub package. In this example, we create a publisher for the chatter topic. This should feel relatively familiar if you’re a ROS veteran. The Publisher publishes std_msgs.String messages to the /chatter topic.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
package org.ros.rosjava_tutorial_pubsub;

import org.ros.concurrent.CancellableLoop;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;

/**
 * A simple {@link Publisher} {@link NodeMain}.
 * 
 * @author damonkohler@google.com (Damon Kohler)
 */
public class Talker extends AbstractNodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return GraphName.of("rosjava_tutorial_pubsub/talker");
  }

  @Override
  public void onStart(final ConnectedNode connectedNode) {
    final Publisher<std_msgs.String> publisher =
        connectedNode.newPublisher("chatter", std_msgs.String._TYPE);
    // This CancellableLoop will be canceled automatically when the node shuts
    // down.
    connectedNode.executeCancellableLoop(new CancellableLoop() {
      private int sequenceNumber;

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

      @Override
      protected void loop() throws InterruptedException {
        std_msgs.String str = publisher.newMessage();
        str.setData("Hello world! " + sequenceNumber);
        publisher.publish(str);
        sequenceNumber++;
        Thread.sleep(1000);
      }
    });
  }
}

Line 28 will probably feel unfamailiar even to ROS veterans. This is one example of rosjava’s asynchornous API. The intent of our Talker class is to publish a hello world message to anyone who will listen once per second. One way to accomplish this is to publish the message and sleep in a loop. However, we don’t want to block the NodeListener.onStart method. So, we create a CancellableLoop and ask the Node to execute it. The loop will be interrupted automatically when the Node exits.

On line 38 we create a new std_msgs.String message to publish using the Publisher.newMessage method. Messages in rosjava cannot be instantiated directly. More on that later.

Now lets take a look at the Listener class.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
package org.ros.rosjava_tutorial_pubsub;

import org.apache.commons.logging.Log;
import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.topic.Subscriber;

/**
 * A simple {@link Subscriber} {@link NodeMain}.
 * 
 * @author damonkohler@google.com (Damon Kohler)
 */
public class Listener extends AbstractNodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return GraphName.of("rosjava_tutorial_pubsub/listener");
  }

  @Override
  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>() {
      @Override
      public void onNewMessage(std_msgs.String message) {
        log.info("I heard: \"" + message.getData() + "\"");
      }
    });
  }
}

In lines 27-32 we see another example of rosjava’s asynchornous API. We can add as many MessageListeners to our Subscriber as we like. When a new message is received, all of our MessageListeners will be called with the incoming message as an argument to MessageListener.onNewMessage.

Executing nodes

When packaging your application into jar, you can use RosRun as the main class. RosRun provides a NodeMainExecutor and a command line interface that will be familiar to ROS veterans. For example, the following steps will build and execute the Talker and Listener nodes in separate processes:

# source your devel/setup.bash
roscd rosjava_core
cd rosjava_tutorial_pubsub
../gradlew installApp
roscore &
./build/install/rosjava_tutorial_pubsub/bin/rosjava_tutorial_pubsub org.ros.rosjava_tutorial_pubsub.Talker &
./build/install/rosjava_tutorial_pubsub/bin/rosjava_tutorial_pubsub org.ros.rosjava_tutorial_pubsub.Listener

Note

The above example launches roscore and the Talker node in the background. You could instead launch each in a separate terminal. Also, you may notice that rosrun cannot find the installed executable. This is a known issue that will be addressed in the future.

At this point, you should see the familiar “Hello, world!” messages start appearing in your terminal. You can also echo the topic using the rostopic command line tool:

rostopic echo chatter

You can configure the executed nodes from the command line in the same way you would any other ROS executable. For example, the following commands will remap the default topic /chatter to /foo.

./build/install/rosjava_tutorial_pubsub/bin/rosjava_tutorial_pubsub org.ros.rosjava_tutorial_pubsub.Talker chatter:=/foo &
./build/install/rosjava_tutorial_pubsub/bin/rosjava_tutorial_pubsub org.ros.rosjava_tutorial_pubsub.Listener chatter:=/foo

See Remapping%20Arguments for more information on passing arguments to ROS executables.

Note

Only the arguments described in Remapping%20Arguments are supported. Support for arbitrary command line arguments (i.e. argv) will be added in the future.

Services

The following class (Server) is available from the rosjava_tutorial_services package. In this example, we create a ServiceServer for the rosjava_test_msgs.AddTwoInts service. This should feel relatively familiar if you’re a ROS veteran.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
package org.ros.rosjava_tutorial_services;

import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.service.ServiceResponseBuilder;
import org.ros.node.service.ServiceServer;

/**
 * This is a simple {@link ServiceServer} {@link NodeMain}.
 * 
 * @author damonkohler@google.com (Damon Kohler)
 */
public class Server extends AbstractNodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return GraphName.of("rosjava_tutorial_services/server");
  }

  @Override
  public void onStart(ConnectedNode connectedNode) {
    connectedNode.newServiceServer("add_two_ints", rosjava_test_msgs.AddTwoInts._TYPE,
        new ServiceResponseBuilder<rosjava_test_msgs.AddTwoIntsRequest, rosjava_test_msgs.AddTwoIntsResponse>() {
          @Override
          public void
              build(rosjava_test_msgs.AddTwoIntsRequest request, rosjava_test_msgs.AddTwoIntsResponse response) {
            response.setSum(request.getA() + request.getB());
          }
        });
  }
}

The ServiceResponseBuilder is called asynchronously for each incoming request. On line 29 we modify the response output parameter to contain the sum of the two integers in the request. The response will be sent once the ServiceResponseBuilder.build returns.

Now lets take a look at the Client class.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
package org.ros.rosjava_tutorial_services;

import org.ros.exception.RemoteException;
import org.ros.exception.RosRuntimeException;
import org.ros.exception.ServiceNotFoundException;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.service.ServiceClient;
import org.ros.node.service.ServiceResponseListener;

/**
 * A simple {@link ServiceClient} {@link NodeMain}.
 * 
 * @author damonkohler@google.com (Damon Kohler)
 */
public class Client extends AbstractNodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return GraphName.of("rosjava_tutorial_services/client");
  }

  @Override
  public void onStart(final ConnectedNode connectedNode) {
    ServiceClient<rosjava_test_msgs.AddTwoIntsRequest, rosjava_test_msgs.AddTwoIntsResponse> serviceClient;
    try {
      serviceClient = connectedNode.newServiceClient("add_two_ints", rosjava_test_msgs.AddTwoInts._TYPE);
    } catch (ServiceNotFoundException e) {
      throw new RosRuntimeException(e);
    }
    final rosjava_test_msgs.AddTwoIntsRequest request = serviceClient.newMessage();
    request.setA(2);
    request.setB(2);
    serviceClient.call(request, new ServiceResponseListener<rosjava_test_msgs.AddTwoIntsResponse>() {
      @Override
      public void onSuccess(rosjava_test_msgs.AddTwoIntsResponse response) {
        connectedNode.getLog().info(
            String.format("%d + %d = %d", request.getA(), request.getB(), response.getSum()));
      }

      @Override
      public void onFailure(RemoteException e) {
        throw new RosRuntimeException(e);
      }
    });
  }
}

In lines 36-47 we see another example of rosjava’s asynchornous API. When the response is received, our ServiceResponseListener will be called with the incoming response as an argument to ServiceResponseListener.onSuccess. In the event that the server thows a ServiceException while building the response, ServiceResponseListener.onFailure will be called. The RemoteException will contain the error message from the server.

Building and executing these nodes works in the same manner as described above:

# source your devel/setup.bash
roscd rosjava_core
cd rosjava_tutorial_services
../gradlew installApp
roscore &
./build/install/rosjava_tutorial_services/bin/rosjava_tutorial_services org.ros.rosjava_tutorial_services.Server &
./build/install/rosjava_tutorial_services/bin/rosjava_tutorial_services org.ros.rosjava_tutorial_services.Client

At this point, you should see the log message “2 + 2 = 4” appear in your terminal. You can also access the service using the rosservice command line tool:

rosservice add_two_ints 2 2

Just as before, you can configure the executed nodes from the command line in the same way you would any other ROS executable. See Remapping%20Arguments for more information on passing arguments to ROS executables.

Messages

Messages are defined as interfaces. Since this makes it impossible to instantiate the message directly, it’s necessary to use a MessageFactory or helper methods such as Publisher.newMessage. This indirection allows the underlying message implementation to change in the future.

Node node;

...

PointCloud2 msg = node.getTopicMessageFactory()
    .newMessage(sensor_msgs.PointCloud._TYPE);

If you want to use messages that you define, whether they are officially released packages or your own custom packages, follow the instructions on the roswiki (refer to rosjava/Messages).

Parameters

rosjava offers full access to the ROS Parameter Server. The Parameter Server is a shared dictionary of configuration parameters accessible to all the nodes at runtime. It is meant to store configuration parameters that are easy to inspect and modify.

Parameters are accessible via ParameterTrees (provided by Nodes).

ParameterTree params = node.newParameterTree();

Accessing Parameters

The ParameterTree API allows you to set and query lists, maps, and single objects of integers, strings and floats.

Unlike typical ROS Client Libraries, rosjava requires that the type of the parameter be known when you retrieve it. If the actual parameter type doesn’t match the expected type, an exception will be thrown.

boolean foo = params.getBoolean("/foo");
int bar = params.getInteger("/bar", 42 /* default value */);
double baz = params.getDouble("/foo/baz");

params.set("/bloop", "Hello, world!");
String helloWorld = params.getString("/bloop");

List<Integer> numbers = params.getList("/numbers");
Map<String, String> strings = params.getMap("/strings");

As with other ROS client libraries, it is possible to retrieve a subtree of parameters. However, you will be responsible for casting the values to their appropriate types.

Map<String, Object> subtree = params.getMap("/subtree");

Using a ParameterListener

It is also possible to subscribe to a particular parameter using a ParameterListener. Note that this does not work for parameter subtrees.

params.addParameterListener("/foo/bar", new ParameterListener() {
  @Override
  public void onNewValue(Object value) {
    ...
  }
});

Currently, ParameterListeners are not generic. Instead, you are responsible for casting the value appropriately.

Logging

The logging interface for rosjava is accessed through Node objects via the Node.getLog method. This object returns an Apache Commons Log object which handles the debug, info, error, warning, and fatal logging outputs for ROS.

node.getLog.debug("debug message");
node.getLog.info(" informative message");

node.getLog.warn("warning message");

// report an error message
node.getLog.error("error message");

// error message with an exception
// so that it can print the stack trace
node.getLog.error("error message", e);

node.fatal("message informing user of a fatal occurrence");

«  Best practices   ::   Contents   ::   Advanced topics  »