@Override public void onStart(ConnectedNode connectedNode) { final Publisher<nav_msgs.Odometry> publisher = connectedNode.newPublisher("odom_echo", nav_msgs.Odometry._TYPE); Subscriber<nav_msgs.Odometry> subscriber = connectedNode.newSubscriber("odom", nav_msgs.Odometry._TYPE); subscriber.addMessageListener(new MessageListener<Odometry>() { @Override public void onNewMessage(Odometry message) { publisher.publish(message); } }); }
@Override public void onStart(ConnectedNode connectedNode) { ParameterTree parameterTree = connectedNode.getParameterTree(); log = connectedNode.getLog(); // TODO: For some reason, setting the / param when using a rosjava master doesn't work // It does work fine with an external master, and also setting other params of any type for (LoadedResource r : params) { addParams(parameterTree, r.namespace, r.resource); } connectedNode.shutdown(); }
connectedNode.newPublisher("chatter_out", std_msgs.String._TYPE); MessageListener<std_msgs.String> chatter_cb = new MessageListener<std_msgs.String>() { @Override connectedNode.newSubscriber("chatter_in", std_msgs.String._TYPE); stringSubscriber.addMessageListener(chatter_cb); connectedNode.newPublisher("int64", std_msgs.Int64._TYPE); MessageListener<std_msgs.Int64> int64_cb = new MessageListener<std_msgs.Int64>() { @Override connectedNode.newSubscriber("int64", "std_msgs/std_msgs.Int64"); int64Subscriber.addMessageListener(int64_cb); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException {
@Override public void onStart(final ConnectedNode connectedNode) { final Publisher<std_msgs.String> publisher = connectedNode.newPublisher(topic_name, std_msgs.String._TYPE); // This CancellableLoop will be canceled automatically when the node shuts // down. connectedNode.executeCancellableLoop(new CancellableLoop() { private int sequenceNumber; @Override protected void setup() { sequenceNumber = 0; } @Override protected void loop() throws InterruptedException { std_msgs.String str = publisher.newMessage(); str.setData("Hello world! " + sequenceNumber); publisher.publish(str); sequenceNumber++; Thread.sleep(1000); } }); } }
@Override public void onStart(ConnectedNode connectedNode) { final Log log = connectedNode.getLog(); Subscriber<std_msgs.String> subscriber = connectedNode.newSubscriber("chatter", std_msgs.String._TYPE); subscriber.addMessageListener(new MessageListener<std_msgs.String>() { @Override public void onNewMessage(std_msgs.String message) { log.info("I heard: \"" + message.getData() + "\""); } }); } }
@SuppressWarnings({ "unchecked", "rawtypes" }) public void onStart(ConnectedNode connectedNode) parameters = connectedNode.getParameterTree(); for (Entry<String, RosTopicSubscriberInterface<? extends Message>> entry : subscribers.entrySet()) continue; Subscriber<? extends Message> subscriber = connectedNode.newSubscriber(entry.getKey(), rosTopicSubscriber.getMessageType()); subscriber.addMessageListener(rosTopicSubscriber); rosSubscribers.put(rosTopicSubscriber, subscriber); Publisher<? extends Message> publisher = connectedNode.newPublisher(entry.getKey(), rosTopicPublisher.getMessageType()); rosTopicPublisher.registered(publisher); rosTopicPublisher.setConnectedNode(connectedNode); ServiceServer server = connectedNode.newServiceServer(entry.getKey(), rosServiceServer.getRequestType(), rosServiceServer); rosServiceServer.setServiceServer(server, connectedNode, entry.getKey()); ServiceClient client = connectedNode.newServiceClient(entry.getKey(), rosServiceClient.getRequestType()); rosServiceClient.setServiceClient(client, connectedNode, entry.getKey()); break; connectedNode.getParameterTree().addParameterListener(entry.getKey(), entry.getValue());
@Override public void onStart(final ConnectedNode connectedNode) { tfSubscriber = connectedNode.newSubscriber("tf", tf2_msgs.TFMessage._TYPE); tfSubscriber.addMessageListener(new MessageListener<tf2_msgs.TFMessage>() { @Override tfPublisher = connectedNode.newPublisher("tf", tf2_msgs.TFMessage._TYPE); final tf2_msgs.TFMessage tfMessage = tfPublisher.newMessage(); geometry_msgs.TransformStamped transformStamped = connectedNode.getTopicMessageFactory().newFromType(geometry_msgs.TransformStamped._TYPE); tfMessage.getTransforms().add(transformStamped); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { time = connectedNode.getCurrentTime(); statusPublisher = connectedNode.newPublisher("status", std_msgs.String._TYPE); final Rate rate = new WallTimeRate(1); final std_msgs.String status = statusPublisher.newMessage(); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException {
protected void setupSubscribers(ConnectedNode connectedNode) { leftEyeImageSubscriber = connectedNode.newSubscriber(STEREO_NAMESPACE + "left/" + IMAGE, CompressedImage._TYPE); rightEyeImageSubscriber = connectedNode.newSubscriber(STEREO_NAMESPACE + "right/" + IMAGE, CompressedImage._TYPE); steeringWheelStateSubscriber = connectedNode.newSubscriber("/drc_vehicle/hand_wheel/state", std_msgs.Float64._TYPE); handBrakeStateSubscriber = connectedNode.newSubscriber("/drc_vehicle/hand_brake/state", std_msgs.Float64._TYPE); gasPedalStateSubscriber = connectedNode.newSubscriber("/drc_vehicle/gas_pedal/state", std_msgs.Float64._TYPE); brakePedalStateSubscriber = connectedNode.newSubscriber("/drc_vehicle/brake_pedal/state", std_msgs.Float64._TYPE); timeSubscriber = connectedNode.newSubscriber("/clock",rosgraph_msgs.Clock._TYPE); }
public void onStart(ConnectedNode connectedNode) { final Publisher<std_msgs.String> pub_tilde = connectedNode.newPublisher("tilde", std_msgs.String._TYPE); final Publisher<std_msgs.String> pub_string = connectedNode.newPublisher("string", std_msgs.String._TYPE); final Publisher<std_msgs.Int64> pub_int = connectedNode.newPublisher("int", std_msgs.Int64._TYPE); final Publisher<std_msgs.Bool> pub_bool = connectedNode.newPublisher("bool", std_msgs.Bool._TYPE); final Publisher<std_msgs.Float64> pub_float = connectedNode.newPublisher("float", std_msgs.Float64._TYPE); final Publisher<rosjava_test_msgs.Composite> pub_composite = connectedNode.newPublisher("composite", rosjava_test_msgs.Composite._TYPE); final Publisher<rosjava_test_msgs.TestArrays> pub_list = connectedNode.newPublisher("list", rosjava_test_msgs.TestArrays._TYPE); ParameterTree param = connectedNode.getParameterTree(); Log log = connectedNode.getLog(); MessageFactory topicMessageFactory = connectedNode.getTopicMessageFactory(); tilde_m.setData(param.getString(connectedNode.resolveName("~tilde").toString())); log.info("tilde: " + tilde_m.getData()); log.info("parameter_namespace: " + paramNamespace); log.info("target_namespace: " + targetNamespace); NameResolver resolver = connectedNode.getResolver().newChild(paramNamespace); NameResolver setResolver = connectedNode.getResolver().newChild(targetNamespace);
private void setupPublishers(ConnectedNode connectedNode) { steeringWheelCommandPublisher = connectedNode.newPublisher("/drc_vehicle/hand_wheel/cmd", std_msgs.Float64._TYPE); handBrakeCommandPublisher = connectedNode.newPublisher("/drc_vehicle/hand_brake/cmd", std_msgs.Float64._TYPE); gasPedalCommandPublisher = connectedNode.newPublisher("/drc_vehicle/gas_pedal/cmd", std_msgs.Float64._TYPE); brakePedalCommandPublisher = connectedNode.newPublisher("/drc_vehicle/brake_pedal/cmd", std_msgs.Float64._TYPE); teleportInToCarPublisher = connectedNode.newPublisher("/drc_world/robot_enter_car", geometry_msgs.Pose._TYPE); teleportOutOfCarPublisher = connectedNode.newPublisher("/drc_world/robot_exit_car", geometry_msgs.Pose._TYPE); directionPublisher = connectedNode.newPublisher("/drc_vehicle/direction/cmd", Int8._TYPE); }
ConnectedNode connectedNode = holder.get(); assertGraphNameEquals("/foo", connectedNode.resolveName("/foo")); assertGraphNameEquals("/ns1/foo", connectedNode.resolveName("foo")); assertGraphNameEquals("/ns1/test_resolver/foo", connectedNode.resolveName("~foo")); Publisher<std_msgs.Int64> pub = connectedNode.newPublisher("pub", std_msgs.Int64._TYPE); assertGraphNameEquals("/ns1/pub", pub.getTopicName()); pub = connectedNode.newPublisher("/pub", std_msgs.Int64._TYPE); assertGraphNameEquals("/pub", pub.getTopicName()); pub = connectedNode.newPublisher("~pub", std_msgs.Int64._TYPE); assertGraphNameEquals("/ns1/test_resolver/pub", pub.getTopicName()); Subscriber<std_msgs.Int64> sub = connectedNode.newSubscriber("sub", std_msgs.Int64._TYPE); assertGraphNameEquals("/ns1/sub", sub.getTopicName()); sub = connectedNode.newSubscriber("/sub", std_msgs.Int64._TYPE); assertGraphNameEquals("/sub", sub.getTopicName()); sub = connectedNode.newSubscriber("~sub", std_msgs.Int64._TYPE); assertGraphNameEquals("/ns1/test_resolver/sub", sub.getTopicName());
@Override public void onStart(final ConnectedNode connectedNode) { connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { time = connectedNode.getCurrentTime(); final Publisher<std_msgs.String> statusPublisher = connectedNode.newPublisher("status", std_msgs.String._TYPE); final Rate rate = new WallTimeRate(1); final std_msgs.String status = statusPublisher.newMessage(); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException {
@Override public void onStart(ConnectedNode connectedNode) { parameters = connectedNode.getParameterTree(); log = connectedNode.getLog(); latch.countDown(); } }, nodeConfiguration);
URI nodeUri = connectedNode.getUri(); assertTrue(nodeUri.getPort() > 0); checkHostName(nodeUri.getHost()); CountDownPublisherListener.newDefault(); Publisher<std_msgs.Int64> publisher = connectedNode.newPublisher("test_addresses_pub", std_msgs.Int64._TYPE); publisher.addListener(publisherListener); assertTrue(publisherListener.awaitMasterRegistrationSuccess(1, TimeUnit.SECONDS));
/** * @param request * @param response */ public void call(T request, ServiceResponseListener<S> response) { checkInitialized(); if (!client.isConnected() || client == null) { if (client != null) client.shutdown(); //locate URI System.err.println("re-connecting to service " + attachedServiceName); try { client = connectedNode.newServiceClient(attachedServiceName, getRequestType()); } catch (ServiceNotFoundException e) { System.err.println("rennection failed. Service not found"); throw new RosRuntimeException(e); } System.err.println("service re-connected, making call"); } client.call(request, response); }
@Override public void onStart(ConnectedNode connectedNode) { parameters = connectedNode.getParameterTree(); latch.countDown(); } }, nodeConfiguration);
@Override protected void loop() throws InterruptedException { geometry_msgs.TransformStamped message = connectedNode.getTopicMessageFactory() .newFromType(geometry_msgs.TransformStamped._TYPE); message.getHeader().setFrameId("world"); message.setChildFrameId("turtle"); message.getHeader().setStamp(connectedNode.getCurrentTime()); message.getTransform().getRotation().setW(Math.random()); message.getTransform().getRotation().setX(Math.random()); message.getTransform().getRotation().setY(Math.random()); message.getTransform().getRotation().setZ(Math.random()); message.getTransform().getTranslation().setX(Math.random()); message.getTransform().getTranslation().setY(Math.random()); message.getTransform().getTranslation().setZ(Math.random()); counter.incrementAndGet(); } });
@Override public void onStart(ConnectedNode connectedNode) { connectedNode.newServiceServer("add_two_ints", rosjava_test_msgs.AddTwoInts._TYPE, new ServiceResponseBuilder<rosjava_test_msgs.AddTwoIntsRequest, rosjava_test_msgs.AddTwoIntsResponse>() { @Override public void build(rosjava_test_msgs.AddTwoIntsRequest request, rosjava_test_msgs.AddTwoIntsResponse response) { response.setSum(request.getA() + request.getB()); } }); } }
public void setConnectedNode(ConnectedNode connectedNode) { this.messageFactory = connectedNode.getTopicMessageFactory(); }
@Override public void onSuccess(rosjava_test_msgs.AddTwoIntsResponse response) { connectedNode.getLog().info( String.format("%d + %d = %d", request.getA(), request.getB(), response.getSum())); }