Skip to content
Snippets Groups Projects
Commit 18219028 authored by Ernesto Corbellini's avatar Ernesto Corbellini
Browse files

Merge branch 'master' into test_client

Conflicts:
	src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
parents c92108ab f0d5d8f7
Branches
No related tags found
No related merge requests found
<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>
......@@ -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)
}
......@@ -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();
}
}
......@@ -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);
}
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) {
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment