@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)); }