@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();
 }
Exemple #18
0
  @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);
 }
Exemple #20
0
 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");
 }
Exemple #22
0
 @Override
 public GraphName getDefaultNodeName() {
   return GraphName.of(C.DefaultBaseNodeName + "/" + QUEUE_NAME);
 }
Exemple #23
0
 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));
 }