Merge branch 'master' into test_client

Conflicts:
	src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java

src/rosjava_actionlib/launch/test.launch

+<launch>
+<node pkg="actionlib_tutorials" type="fibonacci_server" name="fibonacci_server"/>
+<test pkg="rosjava_actionlib" type="execute" test-name="rosjava_actionlib" args="com.github.ekumen.rosjava_actionlib.TestClient"/>
+</launch>

src/rosjava_actionlib/rosjava_actionlib/build.gradle

@@ -37,3 +37,15 @@ dependencies {
 
    compile 'org.ros.rosjava_messages:actionlib_tutorials:[0.1,)'
 }
+
+task deployApp(dependsOn: 'installApp') << {
+  File binDir = new File(project.projectDir, '/bin')
+  if (! binDir.isDirectory()) {
+    println "Creating $binDir directory"
+    binDir.mkdirs()
+  }
+  File link = new File(binDir,"execute")
+  File target = new File(project.projectDir, "build/install/$project.name/bin/$project.name")
+  println "Creating symlink from $link.absolutePath to $target.absolutePath"
+  ant.symlink(link: link.absolutePath, resource: target.absolutePath)
+}

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java

@@ -7,37 +7,86 @@ import org.ros.node.ConnectedNode;
 import org.ros.node.topic.Subscriber;
 import org.ros.node.topic.Publisher;
 import org.ros.message.MessageListener;
-import org.ros.message.Message;
+import org.ros.internal.message.Message;
+import java.util.concurrent.TimeUnit;
+
+public class ActionClient<T_ACTION_GOAL extends Message,
+  T_ACTION_FEEDBACK extends Message,
+  T_ACTION_RESULT extends Message> {
+
+  T_ACTION_GOAL actionGoal;
+  String actionGoalType;
+  String actionResultType;
+  String actionFeedbackType;
+  Publisher<T_ACTION_GOAL> goalPublisher = null;
+  //Publisher<actionlib_msgs.cancel> clientCancel;
+  //Suscriber<actionlib_msgs.status> serverStatus;
+  Subscriber<T_ACTION_RESULT> serverResult = null;
+  Subscriber<T_ACTION_FEEDBACK> serverFeedback = null;
+  ConnectedNode node = null;
+  String actionName;
+  ActionClientListener callbackTarget = null;
+
+  ActionClient (ConnectedNode node, String actionName, String actionGoalType,
+    String actionFeedbackType, String actionResultType) {
+    this.node = node;
+    this.actionName = actionName;
+    this.actionGoalType = actionGoalType;
+    this.actionFeedbackType = actionFeedbackType;
+    this.actionResultType = actionResultType;
 
-public class ActionClient extends AbstractNodeMain {
+    connect(node);
+  }
 
-  private string actionName = "fibonacci";
+  public void attachListener(ActionClientListener target) {
+    callbackTarget = target;
+  }
 
-  //actionlib_tutorials.FibonacciActionGoal actionGoal;
-  Publisher<actionlib_tutorials.FibonacciActionGoal> clientGoal;
-  //Publisher<actionlib_msgs.cancel> clientCancel;
-  //Suscriber<actionlib_msgs.status> serverStatus;
-  Subscriber<actionlib_tutorials.FibonacciActionResult> serverResult;
-  //Suscriber<actionlib_tutorials.FibonacciActionFeedback> serverFeedback;
+  public void sendGoal(T_ACTION_GOAL goal) {
+    goalPublisher.publish(goal);
+  }
 
   void ActionClient(string actionName) {
     this.actionName = actionName;
   }
   
   private void publishClient(ConnectedNode node) {
-    clientGoal = node.newPublisher(actionName + "/goal",
-      actionlib_tutorials.FibonacciActionGoal._TYPE);
+    goalPublisher = node.newPublisher(actionName + "/goal", actionGoalType);
     //clientCancel = connectedNode.newPublisher("fibonacci/cancel",
     //  actionlib_msgs.cancel._TYPE);
   }
 
-  private void suscribeToServer(ConnectedNode node) {
-    serverResult = node.newSubscriber(actionName + "/result",
-      actionlib_tutorials.FibonacciActionResult._TYPE);
+  private void unpublishClient() {
+    if (goalPublisher != null) {
+      goalPublisher.shutdown(5, TimeUnit.SECONDS);
+    }
+  }
+
+  public T_ACTION_GOAL newGoalMessage() {
+    return goalPublisher.newMessage();
+  }
+
+  private void subscribeToServer(ConnectedNode node) {
+    serverResult = node.newSubscriber(actionName + "/result", actionResultType);
+    serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType);
 
-    serverResult.addMessageListener(new MessageListener<actionlib_tutorials.FibonacciActionResult>() {
+    serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() {
       @Override
-      public void onNewMessage(actionlib_tutorials.FibonacciActionResult message) {
+      public void onNewMessage(T_ACTION_FEEDBACK message) {
+        gotFeedback(message);
+      }
+    });
+
+    serverResult.addMessageListener(new MessageListener<T_ACTION_RESULT>() {
+      @Override
+      public void onNewMessage(T_ACTION_RESULT message) {
+        gotResult(message);
+      }
+    });
+
+    serverResult.addMessageListener(new MessageListener<T_ACTION_RESULT>() {
+      @Override
+      public void onNewMessage(T_ACTION_RESULT message) {
         gotResult(message);
       }
     });
@@ -48,51 +97,43 @@ public class ActionClient extends AbstractNodeMain {
       actionlib_tutorials.FibonacciActionFeedback._TYPE);*/
   }
 
-  public void gotResult(actionlib_tutorials.FibonacciActionResult message) {
-    actionlib_tutorials.FibonacciResult result = message.getResult();
-    int[] sequence = result.getSequence();
-    int i;
+  private void unsubscribeToServer() {
+    if (serverFeedback != null) {
+      serverFeedback.shutdown(5, TimeUnit.SECONDS);
+    }
+    if (serverResult != null) {
+      serverResult.shutdown(5, TimeUnit.SECONDS);
+    }
+  }
 
-    System.out.print("Got Fibonacci result sequence! ");
-    for (i=0; i<sequence.length; i++)
-      System.out.print(Integer.toString(sequence[i]) + " ");
-    System.out.print("\n");
+  public void gotResult(T_ACTION_RESULT message) {
+    // Propagate the callback
+    if (callbackTarget != null) {
+      callbackTarget.resultReceived(message);
+    }
+  }
+
+  public void gotFeedback(T_ACTION_FEEDBACK message) {
+    // Propagate the callback
+    if (callbackTarget != null) {
+      callbackTarget.feedbackReceived(message);
+    }
   }
 
   /**
   * Publishes the client's topics and suscribes to the server's topics.
   */
-  public void connect(ConnectedNode node) {
+  private void connect(ConnectedNode node) {
     publishClient(node);
-    //suscribeServer(node);
-  }
-
-  @Override
-  public GraphName getDefaultNodeName() {
-      return GraphName.of("fibonacci_client");
+    subscribeToServer(node);
   }
 
-  @Override
-  public void onStart(ConnectedNode node) {
-    connect(node);
-
-    suscribeServer(node);
-
-    // publish a goal message
-    actionlib_tutorials.FibonacciActionGoal goalMessage = clientGoal.newMessage();
-    actionlib_tutorials.FibonacciGoal fibonacciGoal = goalMessage.getGoal();
-    // set Fibonacci parameter
-    fibonacciGoal.setOrder(6);
-    goalMessage.setGoal(fibonacciGoal);
-
-    while (true) {
-      clientGoal.publish(goalMessage);
-      try {
-        Thread.sleep(10000);
-      }
-      catch (InterruptedException ex) {
-        ;
-      }
-    }
+  /**
+   * Finish the action client. Unregister publishers and listeners.
+   */
+  public void finish() {
+    callbackTarget = null;
+    unpublishClient();
+    unsubscribeToServer();
   }
 }

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClientListener.java

@@ -4,9 +4,11 @@ import org.ros.internal.message.Message;
 
 /**
  * Listener interface to receive the incoming messages from the ActionLib server.
+ * A client should implement this interface if it wants to receive the callbacks
+ * with information from the server.
  */
-public interface ActionClientListener {
-  void resultReceived(Message result);
-  void feedbackReceived(Message feedback);
+public interface ActionClientListener<T_ACTION_FEEDBACK extends Message, T_ACTION_RESULT extends Message> {
+  void resultReceived(T_ACTION_RESULT result);
+  void feedbackReceived(T_ACTION_FEEDBACK feedback);
   void statusReceived(Message status);
 }

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java

+package com.github.ekumen.rosjava_actionlib;
+
+import org.ros.namespace.GraphName;
+import org.ros.node.AbstractNodeMain;
+import org.ros.node.ConnectedNode;
+import org.ros.internal.message.Message;
+import actionlib_tutorials.FibonacciActionGoal;
+import actionlib_tutorials.FibonacciActionFeedback;
+import actionlib_tutorials.FibonacciActionResult;
+import actionlib_tutorials.FibonacciGoal;
+import actionlib_tutorials.FibonacciFeedback;
+import actionlib_tutorials.FibonacciResult;
+
+public class TestClient extends AbstractNodeMain implements ActionClientListener<FibonacciActionFeedback, FibonacciActionResult> {
+  ActionClient ac;
+  volatile private boolean responded = false;
+
+  @Override
+  public GraphName getDefaultNodeName() {
+    return GraphName.of("fibonacci_test_client");
+  }
+
+  @Override
+  public void onStart(ConnectedNode node) {
+    ac = new ActionClient<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE, FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE);
+    int repeat = 3;
+
+    // Attach listener for the callbacks
+    ac.attachListener(this);
+
+    // publish a goal message
+    FibonacciActionGoal goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
+    FibonacciGoal fibonacciGoal = goalMessage.getGoal();
+
+    // set Fibonacci parameter
+    fibonacciGoal.setOrder(6);
+    goalMessage.setGoal(fibonacciGoal);
+
+    while(repeat > 0) {
+      sleep(1000);
+    System.out.println("Sending goal #" + repeat + "...");
+    ac.sendGoal(goalMessage);
+    System.out.println("Goal sent.");
+    repeat--;
+    }
+
+    while (repeat > 0) {
+      responded = false;
+      ac.sendGoal(goalMessage);
+      while (!responded) {
+      }
+      repeat--;
+    }
+
+    System.out.println("Finishing node!!");
+    sleep(30000);
+    ac.finish();
+    node.shutdown();
+  }
+
+  @Override
+  public void resultReceived(FibonacciActionResult message) {
+    FibonacciResult result = message.getResult();
+    int[] sequence = result.getSequence();
+    int i;
+
+    responded = true;
+
+    System.out.print("Got Fibonacci result sequence! ");
+    for (i=0; i<sequence.length; i++)
+      System.out.print(Integer.toString(sequence[i]) + " ");
+    System.out.print("\n");
+  }
+
+  @Override
+  public void feedbackReceived(FibonacciActionFeedback message) {
+    FibonacciFeedback result = message.getFeedback();
+    int[] sequence = result.getSequence();
+    int i;
+
+    System.out.print("Feedback from Fibonacci server: ");
+    for (i=0; i<sequence.length; i++)
+      System.out.print(Integer.toString(sequence[i]) + " ");
+    System.out.print("\n");
+  }
+
+  @Override
+  public void statusReceived(Message status) {
+
+  }
+
+  private void sleep(long msec) {
+    try {
+      Thread.sleep(msec);
+    }
+    catch (InterruptedException ex) {
+    }
+  }
+}