@Override
public List<Object> registerSubscriber(
String callerId,
String topicName,
String topicMessageType,
String callerSlaveUri,
String port) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"registerSubscriber",
GraphName.of(callerId),
new InetSocketAddress(callerSlaveUri, Integer.valueOf(port)),
GraphName.of(topicName),
topicMessageType);
List<InetSocketAddress> publishers =
// master.registerSubscriber(GraphName.of(callerId), new InetSocketAddress(callerSlaveUri,
// Integer.valueOf(port)),
// GraphName.of(topicName), topicMessageType);
(List<InetSocketAddress>) remoteMaster.queue(rri);
// List<String> urls = new ArrayList<String>();
// for (InetSocketAddress uri : publishers) {
// urls.add(uri.toString());
// }
return Response.newSuccess("Success", publishers).toList();
}
public Response<GraphName> searchParam(GraphName parameterName) {
Response<String> response =
Response.fromListCheckedFailure(
rpcEndpoint.searchParam(nodeName, parameterName.toString()), new StringResultFactory());
return new Response<GraphName>(
response.getStatusCode(),
response.getStatusMessage(),
GraphName.of((String) response.getResult()));
}
@Override
public List<Object> getPublishedTopics(String callerId, String subgraph) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"getPublishedTopics",
GraphName.of(callerId),
GraphName.of(subgraph));
return Response.newSuccess(
"current topics", remoteMaster.queue(rri)
// master.getPublishedTopics(GraphName.of(callerId), GraphName.of(subgraph))
)
.toList();
}
@Override
public List<Object> unregisterService(
String callerId, String serviceName, String serviceUri, String servicePort) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"unregisterService",
GraphName.of(callerId),
GraphName.of(serviceName),
new InetSocketAddress(serviceUri, Integer.valueOf(servicePort)));
boolean result =
// master.unregisterService(GraphName.of(callerId), GraphName.of(serviceName), new
// InetSocketAddress(serviceUri, Integer.valueOf(servicePort)));
(boolean) remoteMaster.queue(rri);
return Response.newSuccess("Success", result ? 1 : 0).toList();
}
@Override
public List<Object> unregisterSubscriber(
String callerId, String topicName, String callerSlaveUri, String port) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"unregisterSubscriber",
GraphName.of(callerId),
GraphName.of(topicName));
boolean result =
(boolean)
remoteMaster.queue(
rri); // master.unregisterSubscriber(GraphName.of(callerId),
// GraphName.of(topicName));
return Response.newSuccess("Success", result ? 1 : 0).toList();
}
@Override
public List<Object> deleteParam(String callerId, String key) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.ParameterServer", "delete", GraphName.of(key));
// parameterServer.delete(GraphName.of(key));
return Response.newSuccess("Success", remoteParameter.queue(rri)).toList();
}
public Response<Integer> unsubscribeParam(GraphName parameterName) {
return Response.fromListChecked(
rpcEndpoint.unsubscribeParam(
nodeName,
nodeIdentifier.getUri().getHostName(),
String.valueOf(nodeIdentifier.getUri().getPort()),
parameterName.toString()),
new IntegerResultFactory());
}
@Override
public List<Object> getParam(String callerId, String key) {
RemoteRequestInterface rri =
new RemoteRequest("org.ros.internal.node.server.ParameterServer", "get", GraphName.of(key));
Object value = remoteParameter.queue(rri); // parameterServer.get(GraphName.of(key));
if (value == null) {
return Response.newError("Parameter \"" + key + "\" is not set.", null).toList();
}
return Response.newSuccess("Success", value).toList();
}
private void add(GraphName source, LazyFrameTransform lazyFrameTransform) {
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
if (!transforms.containsKey(relativeSource)) {
transforms.put(
relativeSource, new CircularBlockingDeque<LazyFrameTransform>(TRANSFORM_QUEUE_CAPACITY));
}
synchronized (mutex) {
transforms.get(relativeSource).addFirst(lazyFrameTransform);
}
}
public Response<Boolean> hasParam(GraphName parameterName) {
if (DEBUG)
log.debug(
"ParameterClient.hasParam name:"
+ parameterName
+ " node "
+ nodeName
+ " for RPC endpoint:"
+ rpcEndpoint);
return Response.fromListChecked(
rpcEndpoint.hasParam(nodeName, parameterName.toString()), new BooleanResultFactory());
}
/**
* @return the {@link FrameTransform} from source the frame to the target frame, or {@code null}
* if no {@link FrameTransform} could be found
*/
public FrameTransform transform(GraphName source, GraphName target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
GraphName relativeTarget = target.toRelative();
if (relativeSource.equals(relativeTarget)) {
return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null);
}
FrameTransform sourceToRoot = transformToRoot(relativeSource);
FrameTransform targetToRoot = transformToRoot(relativeTarget);
if (sourceToRoot == null && targetToRoot == null) {
return null;
}
if (sourceToRoot == null) {
if (targetToRoot.getTargetFrame().equals(relativeSource)) {
// relativeSource is root.
return targetToRoot.invert();
} else {
return null;
}
}
if (targetToRoot == null) {
if (sourceToRoot.getTargetFrame().equals(relativeTarget)) {
// relativeTarget is root.
return sourceToRoot;
} else {
return null;
}
}
if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
// Neither relativeSource nor relativeTarget is root and both share the
// same root.
Transform transform =
targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime());
}
// No known transform.
return null;
}
@Override
public List<Object> registerPublisher(
String callerId,
String topicName,
String topicMessageType,
String callerSlaveUri,
String port) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"registerPublisher",
GraphName.of(callerId),
new InetSocketAddress(callerSlaveUri, Integer.valueOf(port)),
GraphName.of(topicName),
topicMessageType);
List<InetSocketAddress> subscribers =
// master.registerPublisher(GraphName.of(callerId),
// new InetSocketAddress(callerSlaveUri, Integer.valueOf(port)), GraphName.of(topicName),
// topicMessageType);
(List<InetSocketAddress>) remoteMaster.queue(rri);
return Response.newSuccess("Success", subscribers).toList();
}
@Override
public List<Object> registerService(
String callerId,
String serviceName,
String serviceUri,
String serviceport,
String callerSlaveUri,
String callerport) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"registerService",
GraphName.of(callerId),
new InetSocketAddress(callerSlaveUri, Integer.valueOf(callerport)),
GraphName.of(serviceName),
new InetSocketAddress(serviceUri, Integer.valueOf(serviceport)));
// master.registerService(GraphName.of(callerId), new InetSocketAddress(callerSlaveUri,
// Integer.valueOf(callerport)), GraphName.of(
// serviceName), new InetSocketAddress(serviceUri, Integer.valueOf(serviceport)));
return Response.newSuccess("Success", remoteMaster.queue(rri)).toList();
}
@Override
public List<Object> getTopicTypes(String callerId) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"getTopicTypes",
GraphName.of(callerId));
return Response.newSuccess(
"topic types", remoteMaster.queue(rri)
// master.getTopicTypes(GraphName.of(callerId))
)
.toList();
}
@Override
public List<Object> lookupNode(String callerId, String nodeName) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.master.MasterServer",
"lookupNode",
GraphName.of(nodeName));
InetSocketAddress nodeSlaveUri =
(InetSocketAddress) remoteMaster.queue(rri); // master.lookupNode(GraphName.of(nodeName));
if (nodeSlaveUri != null) {
return Response.newSuccess("Success", nodeSlaveUri.toString()).toList();
} else {
return Response.newError("No such node", null).toList();
}
}
@Test
public void testGetPublications() {
SlaveServer mockSlave = mock(SlaveServer.class);
DefaultPublisher<?> mockPublisher = mock(DefaultPublisher.class);
when(mockSlave.getPublications())
.thenReturn(Lists.<DefaultPublisher<?>>newArrayList(mockPublisher));
when(mockPublisher.getTopicName()).thenReturn(GraphName.of("/bar"));
when(mockPublisher.getTopicMessageType()).thenReturn("/baz");
when(mockPublisher.getTopicDeclarationAsList()).thenReturn(Lists.newArrayList("/bar", "/baz"));
SlaveXmlRpcEndpointImpl slave = new SlaveXmlRpcEndpointImpl(mockSlave);
List<Object> response = slave.getPublications("/foo");
assertEquals(StatusCode.SUCCESS.toInt(), response.get(0));
List<List<String>> protocols = Lists.newArrayList();
protocols.add(Lists.newArrayList("/bar", "/baz"));
assertEquals(protocols, response.get(2));
}
@Override
public List<Object> subscribeParam(
String callerId, String callerSlaveUri, String slavePort, String key) {
RemoteRequestInterface rri =
new RemoteRequest(
"org.ros.internal.node.server.ParameterServer",
"subscribe",
GraphName.of(key),
NodeIdentifier.forNameAndUri(callerId, callerSlaveUri, Integer.valueOf(slavePort)));
// parameterServer.subscribe(GraphName.of(key),
// NodeIdentifier.forNameAndUri(callerId, callerSlaveUri, Integer.valueOf(slavePort)));
Object value = remoteParameter.queue(rri); // parameterServer.get(GraphName.of(key));
if (value == null) {
// Must return an empty map as the value of an unset parameter.
value = new HashMap<String, Object>();
}
return Response.newSuccess("Success", value).toList();
}
@Override
public void onStart(ConnectedNode connectedNode) {
this.connectedNode = connectedNode;
super.onStart(connectedNode);
odoComputer = new OdoComputer(1.7, 14.3, connectedNode.getCurrentTime());
ultasonicPublisher = connectedNode.newPublisher(namespace.join("distance"), Float32._TYPE);
voltagePublisher = connectedNode.newPublisher(namespace.join("voltage"), Float32._TYPE);
odometryPublisher = connectedNode.newPublisher(namespace.join("odom"), Odometry._TYPE);
tfPublisher = connectedNode.newPublisher(namespace.join("tf"), TransformStamped._TYPE);
Subscriber<Twist> motorSpeedSubscriber =
connectedNode.newSubscriber(namespace.join("cmd_vel"), Twist._TYPE);
motorSpeedSubscriber.addMessageListener(
new MessageListener<Twist>() {
@Override
public void onNewMessage(Twist msg) {
double x = msg.getLinear().getX();
double y = msg.getAngular().getZ();
if (Math.abs(x) < 0.02) x = 0;
if (Math.abs(y) < 0.02) y = 0;
double s = x < 0 ? -1 : 1;
double speed = Math.sqrt(x * x + y * y) / Math.sqrt(2);
double twist = -200 * y / speed;
speed *= 100 * s;
try {
brick.MOTOR.steerMotors(NXT.Motor.A, NXT.Motor.B, (int) speed, (int) twist, 10000);
} catch (IOException e) {
NXTNode.this.connectedNode.getLog().error("Motor error", e);
}
}
});
Subscriber<Int16> toneSubscriber =
connectedNode.newSubscriber(namespace.join("tone"), Int16._TYPE);
toneSubscriber.addMessageListener(
new MessageListener<Int16>() {
@Override
public void onNewMessage(Int16 freq) {
try {
brick.SYSTEM.playTone(freq.getData(), 500);
} catch (IOException e) {
NXTNode.this.connectedNode.getLog().error("Tone error", e);
}
}
});
connectedNode
.getScheduledExecutorService()
.scheduleAtFixedRate(new SensorSample(), loopMs, loopMs, TimeUnit.MILLISECONDS);
}
private DefaultServiceClient(
GraphName nodeName,
ServiceDeclaration serviceDeclaration,
MessageFactory messageFactory,
ScheduledExecutorService executorService)
throws IOException {
this.serviceDeclaration = serviceDeclaration;
this.messageFactory = messageFactory;
messageBufferPool = new MessageBufferPool();
responseListeners = new LinkedList<ServiceResponseListener<S>>();
connectionHeader = new ConnectionHeader();
connectionHeader.addField(ConnectionHeaderFields.CALLER_ID, nodeName.toString());
// TODO(damonkohler): Support non-persistent connections.
connectionHeader.addField(ConnectionHeaderFields.PERSISTENT, "1");
connectionHeader.merge(serviceDeclaration.toConnectionHeader());
tcpClientManager = TcpClientManager.getInstance(executorService);
ServiceClientHandshakeHandler<T, S> serviceClientHandshakeHandler =
new ServiceClientHandshakeHandler<T, S>(
connectionHeader, responseListeners, executorService);
handshakeLatch = new HandshakeLatch();
serviceClientHandshakeHandler.addListener(handshakeLatch);
tcpClientManager.addNamedChannelHandler(serviceClientHandshakeHandler);
}
private void updateVertexBuffer(nav_msgs.Path path) {
ByteBuffer goalVertexByteBuffer =
ByteBuffer.allocateDirect(path.getPoses().size() * 3 * Float.SIZE / 8);
goalVertexByteBuffer.order(ByteOrder.nativeOrder());
vertexBuffer = goalVertexByteBuffer.asFloatBuffer();
if (path.getPoses().size() > 0) {
frame = GraphName.of(path.getPoses().get(0).getHeader().getFrameId());
// Path poses are densely packed and will make the path look like a solid
// line even if it is drawn as points. Skipping poses provides the visual
// point separation were looking for.
int i = 0;
for (PoseStamped pose : path.getPoses()) {
// TODO(damonkohler): Choose the separation between points as a pixel
// value. This will require inspecting the zoom level from the camera.
if (i % 15 == 0) {
vertexBuffer.put((float) pose.getPose().getPosition().getX());
vertexBuffer.put((float) pose.getPose().getPosition().getY());
vertexBuffer.put((float) pose.getPose().getPosition().getZ());
}
i++;
}
}
vertexBuffer.position(0);
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("android/key");
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of(C.DefaultBaseNodeName + "/" + QUEUE_NAME);
}
public PathLayer(String topic) {
this(GraphName.of(topic));
}
public Response<Object> getParam(GraphName parameterName) {
return Response.fromListCheckedFailure(
rpcEndpoint.getParam(nodeName, parameterName.toString()), new ObjectResultFactory());
}
public Response<Void> setParam(GraphName parameterName, Object parameterValue) {
return Response.fromListChecked(
rpcEndpoint.setParam(nodeName, parameterName.toString(), parameterValue),
new VoidResultFactory());
}
public Response<Void> deleteParam(GraphName parameterName) {
return Response.fromListChecked(
rpcEndpoint.deleteParam(nodeName, parameterName.toString()), new VoidResultFactory());
}
/**
* Returns the most recent {@link FrameTransform} for target {@code source}.
*
* @param source the frame to look up
* @return the most recent {@link FrameTransform} for {@code source} or {@code null} if no
* transform for {@code source} is available
*/
public FrameTransform lookUp(GraphName source) {
Preconditions.checkNotNull(source);
// This adds support for tf2 while maintaining backward compatibility with tf.
return getLatest(source.toRelative());
}
/** @see #lookUp(GraphName, Time) */
public FrameTransform get(String source, Time time) {
Preconditions.checkNotNull(source);
return lookUp(GraphName.of(source), time);
}
/**
* Updates the tree with the provided {@link geometry_msgs.TransformStamped} message.
*
* <p>Note that the tree is updated lazily. Modifications to the provided {@link
* geometry_msgs.TransformStamped} message may cause unpredictable results.
*
* @param transformStamped the {@link geometry_msgs.TransformStamped} message to update with
*/
public void update(geometry_msgs.TransformStamped transformStamped) {
Preconditions.checkNotNull(transformStamped);
GraphName source = GraphName.of(transformStamped.getChildFrameId());
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
add(source, lazyFrameTransform);
}
/** @see #transform(GraphName, GraphName) */
public FrameTransform transform(String source, String target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
return transform(GraphName.of(source), GraphName.of(target));
}
