@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();
}
@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<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> 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();
}
@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> 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> 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> 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();
}
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);
}
/** @see #setFrame(GraphName) */
public void setFrame(String frame) {
setFrame(GraphName.of(frame));
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of(C.DefaultBaseNodeName + "/" + QUEUE_NAME);
}
public PathLayer(String topic) {
this(GraphName.of(topic));
}
public GraphName getDefaultNodeName() {
return GraphName.of("android_sensors_driver/fluid_pressure_publisher");
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("android_15/distance_view");
}
@Override
public void onStart(final ConnectedNode connectedNode) {
Map<GraphName, GraphName> remaps = connectedNode.getResolver().getRemappings();
System.out.println("my namespace is: " + connectedNode.getResolver().getNamespace());
if (remaps.containsKey("noIns")) {
System.out.println("noooooo iiiiiiins " + remaps.get("noIns"));
}
ParameterTree pt = connectedNode.getParameterTree();
ParameterTreeCrawler ptc = new ParameterTreeCrawler(pt);
ptc.printAll();
if (pt.has("noIns")) {
System.out.println("fffffffffff " + pt.getInteger("noIns"));
}
System.out.println(ptc.getAllRemapps(remaps));
System.out.println("reeeeeee " + GraphName.of("ccc").isRelative());
// System.out.println("tried to get param from my NS:
// "+pt.getString(GraphName.of("ccc").toGlobal()));
System.out.println("--a is: " + GraphName.of("b"));
System.out.println("--a is: " + GraphName.of("~b"));
// System.out.println("--a is: "+GraphName.of("_b"));
// System.out.println("--a is: "+GraphName.of("__b"));
// connectedNode.getResolver().getNamespace()
log = connectedNode.getLog();
/*
// register subscribers
subscriberA = connectedNode.newSubscriber(aT, std_msgs.Bool._TYPE);
subscriberA.addMessageListener(new MessageListener<std_msgs.Bool>() {
@Override
public void onNewMessage(Bool message) {
a = message.getData();
//y = copute(a);
send();
}
});// dynamic
// register publisher
publisher = connectedNode.newPublisher(yT, std_msgs.Bool._TYPE);
inited = true;
// infinite loop
connectedNode.executeCancellableLoop(new CancellableLoop() {
@Override
protected void setup() {
}
@Override
protected void loop() throws InterruptedException {
if(SEND){
std_msgs.Bool out = publisher.newMessage();
out.setData(y);
publisher.publish(out);
log.info("Publishing this: \"" + out.getData() + " !! on topic: "+yT);
}
Thread.sleep(sleepTime);
}
});*/
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("cooonf");
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("tfjava_listener");
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("android/key");
}
/** @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));
}
/** @see #lookUp(GraphName, Time) */
public FrameTransform get(String source, Time time) {
Preconditions.checkNotNull(source);
return lookUp(GraphName.of(source), time);
}
/** @see #jumpToFrame(GraphName) */
public void jumpToFrame(String frame) {
jumpToFrame(GraphName.of(frame));
}
/**
* 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);
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("/android_ros_sender");
}
@Override
public void run() {
try {
// Check if the concert exists by looking for concert name parameter
// getParam throws when it can't find the parameter.
ParameterClient paramClient =
new ParameterClient(
NodeIdentifier.forNameAndUri("/concert_checker", concertUri.toString()),
concertUri);
String name = (String) paramClient.getParam(GraphName.of(CONCERT_NAME_PARAM)).getResult();
Log.i("ConcertRemocon", "Concert " + name + " found; retrieve additional information");
NodeMainExecutorService nodeMainExecutorService = new NodeMainExecutorService();
NodeConfiguration nodeConfiguration =
NodeConfiguration.newPublic(
InetAddressFactory.newNonLoopback().getHostAddress(), concertUri);
// Check for the concert information topic
ListenerNode<concert_msgs.ConcertInfo> readInfoTopic =
new ListenerNode(CONCERT_INFO_TOPIC, concert_msgs.ConcertInfo._TYPE);
nodeMainExecutorService.execute(
readInfoTopic, nodeConfiguration.setNodeName("read_info_node"));
readInfoTopic.waitForResponse();
concert_msgs.ConcertInfo concertInfo = readInfoTopic.getLastMessage();
String concertName = concertInfo.getName();
String concertDesc = concertInfo.getDescription();
rocon_std_msgs.Icon concertIcon = concertInfo.getIcon();
if (name.equals(concertName) == false)
Log.w(
"ConcertRemocon", "Concert names from parameter and topic differs; we use the later");
// Check for the concert roles topic
ListenerNode<concert_msgs.Roles> readRolesTopic =
new ListenerNode(CONCERT_ROLES_TOPIC, concert_msgs.Roles._TYPE);
nodeMainExecutorService.execute(
readRolesTopic, nodeConfiguration.setNodeName("concert_roles_node"));
readRolesTopic.waitForResponse();
nodeMainExecutorService.shutdownNodeMain(readInfoTopic);
nodeMainExecutorService.shutdownNodeMain(readRolesTopic);
// configure concert description
Date timeLastSeen = new Date();
ConcertDescription description =
new ConcertDescription(masterId, concertName, concertDesc, concertIcon, timeLastSeen);
Log.i("ConcertRemocon", "Concert is available");
description.setConnectionStatus(ConcertDescription.OK);
description.setUserRoles(readRolesTopic.getLastMessage());
foundConcertCallback.receive(description);
return;
} catch (RuntimeException e) {
// thrown if concert could not be found in the getParam call (from
// java.net.ConnectException)
Log.w(
"ConcertRemocon", "could not find concert [" + concertUri + "][" + e.toString() + "]");
failureCallback.handleFailure(e.toString());
} catch (Throwable e) {
Log.w(
"ConcertRemocon",
"exception while creating node in concert checker for URI " + concertUri,
e);
failureCallback.handleFailure(e.toString());
}
}
public GraphName getDefaultNodeName() {
return GraphName.of(
"darpaRoboticsChallenge/RosConnectedZeroPoseRobotConfigurationDataProducer");
}
