Skip to content

Commit 2d62557

Browse files
neocoretechsneocoretechs
committed
Fully operational bus. Services yet to be fully tested but full Master/Slave/Parameter/TcpRos bidirectional comm
1 parent 3941c26 commit 2d62557

File tree

8 files changed

+87
-45
lines changed

8 files changed

+87
-45
lines changed

‎src/main/java/org/ros/address/AdvertiseAddress.java‎

Lines changed: 28 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ public class AdvertiseAddress implements Serializable {
4242
private String host;
4343
private int port;
4444

45-
private Callable<Integer> portCallable;
45+
//private transient Callable<Integer> portCallable;
4646

4747
public AdvertiseAddress() { }
4848

@@ -87,6 +87,7 @@ public String getHost() {
8787
return host;
8888
}
8989

90+
/*
9091
public void setStaticPort(final int port) {
9192
portCallable = new Callable<Integer>() {
9293
@Override
@@ -95,19 +96,22 @@ public Integer call() throws Exception {
9596
}
9697
};
9798
}
98-
99+
*/
99100
public int getPort() {
100-
try {
101-
return portCallable.call();
102-
} catch (Exception e) {
101+
//try {
102+
// return portCallable.call();
103+
//} catch (Exception e) {
103104
return port;
104105
//throw new RosRuntimeException(e);
105-
}
106+
//}
106107
}
107108

108-
public void setPortCallable(Callable<Integer> portCallable) {
109-
this.portCallable = portCallable;
109+
public void setPort(int port) {
110+
this.port = port;
110111
}
112+
// public void setPortCallable(Callable<Integer> portCallable) {
113+
// this.portCallable = portCallable;
114+
// }
111115

112116
/*
113117
public InetAddress toInetAddress() {
@@ -116,19 +120,19 @@ public InetAddress toInetAddress() {
116120
*/
117121

118122
public InetSocketAddress toInetSocketAddress() {
119-
assert(portCallable != null);
120-
try {
121-
return new InetSocketAddress(host, portCallable.call()); //toInetAddress()
122-
} catch (Exception e) {
123+
//assert(portCallable != null);
124+
//try {
125+
// return new InetSocketAddress(host, portCallable.call()); //toInetAddress()
126+
// } catch (Exception e) {
123127
//throw new RosRuntimeException(e);
124128
return new InetSocketAddress(host, port); //toInetAddress()
125-
}
129+
//}
126130
}
127131

128132
public URI toUri(String scheme) {
129-
assert(portCallable != null);
133+
//assert(portCallable != null);
130134
try {
131-
return new URI(scheme, null, host, portCallable.call(), "/", null, null);
135+
return new URI(scheme, null, host, port/*portCallable.call()*/, "/", null, null);
132136
} catch (Exception e) {
133137
throw new RosRuntimeException("Failed to create URI: " + this, e);
134138
}
@@ -140,22 +144,22 @@ public boolean isLoopbackAddress() {
140144

141145
@Override
142146
public String toString() {
143-
assert(portCallable != null);
147+
//assert(portCallable != null);
144148
try {
145-
return "AdvertiseAddress<" + host + ", " + portCallable.call() + ">";
149+
return "AdvertiseAddress<" + host + ", " + port/*portCallable.call()*/ + ">";
146150
} catch (Exception e) {
147151
return "AdvertiseAddress<" + host + ", " + port + ">";
148152
}
149153
}
150154

151155
@Override
152156
public int hashCode() {
153-
assert(portCallable != null);
157+
//assert(portCallable != null);
154158
final int prime = 31;
155159
int result = 1;
156160
result = prime * result + ((host == null) ? 0 : host.hashCode());
157161
try {
158-
result = prime * result + portCallable.call();
162+
result = prime * result + port;//portCallable.call();
159163
} catch (Exception e) {
160164
throw new RosRuntimeException(e);
161165
}
@@ -164,7 +168,7 @@ public int hashCode() {
164168

165169
@Override
166170
public boolean equals(Object obj) {
167-
assert(portCallable != null);
171+
// assert(portCallable != null);
168172
if (this == obj)
169173
return true;
170174
if (obj == null)
@@ -178,12 +182,14 @@ public boolean equals(Object obj) {
178182
} else if (!host.equals(other.host))
179183
return false;
180184
try {
181-
if (portCallable.call() != other.portCallable.call())
182-
return false;
185+
//if (portCallable.call() != other.portCallable.call())
186+
if( port != other.port)
187+
return false;
183188
} catch (Exception e) {
184189
throw new RosRuntimeException(e);
185190
}
186191
return true;
187192
}
188193

194+
189195
}

‎src/main/java/org/ros/address/PublicAdvertiseAddressFactory.java‎

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,10 @@
1616

1717
package org.ros.address;
1818

19+
import java.io.IOException;
1920
import java.net.InetAddress;
2021
import java.net.InetSocketAddress;
22+
import java.net.ServerSocket;
2123
import java.net.UnknownHostException;
2224

2325
import org.ros.exception.RosRuntimeException;
@@ -54,7 +56,18 @@ public AdvertiseAddress newDefault(int port) {
5456

5557
@Override
5658
public AdvertiseAddress newDefault() {
57-
InetSocketAddress isa = InetSocketAddressFactory.newLoopback();
58-
return new AdvertiseAddress(isa);
59+
try {
60+
return new AdvertiseAddress(host, findPortOnAddress(host));
61+
} catch (IOException e) {
62+
throw new RosRuntimeException("Cannot generate new default advertise address due to "+e);
63+
}
64+
}
65+
66+
private static Integer findPortOnAddress(InetAddress host) throws IOException {
67+
try (
68+
ServerSocket socket = new ServerSocket(0,0,host);
69+
) {
70+
return socket.getLocalPort();
71+
}
5972
}
6073
}

‎src/main/java/org/ros/internal/loader/CommandLineLoader.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,7 @@ private NameResolver buildParentResolver() {
172172
* </ol>
173173
*/
174174
private String getHost() {
175-
String host = InetSocketAddressFactory.newLoopback().getAddress().getCanonicalHostName();
175+
String host = InetSocketAddressFactory.newNonLoopback().getCanonicalHostName();
176176
if (specialRemappings.containsKey(CommandLineVariables.ROS_IP)) {
177177
host = specialRemappings.get(CommandLineVariables.ROS_IP);
178178
} else if (environment.containsKey(EnvironmentVariables.ROS_IP)) {

‎src/main/java/org/ros/internal/node/response/ProtocolDescriptionResultFactory.java‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616

1717
package org.ros.internal.node.response;
1818

19+
import java.util.ArrayList;
1920
import java.util.Arrays;
2021
import java.util.List;
2122

@@ -32,10 +33,11 @@ public class ProtocolDescriptionResultFactory implements ResultFactory<ProtocolD
3233

3334
@Override
3435
public ProtocolDescription newFromValue(Object value) {
35-
List<Object> protocolParameters = Arrays.asList((Object[]) value);
36-
assert(protocolParameters.size() == 3);
37-
assert(protocolParameters.get(0).equals(ProtocolNames.TCPROS));
38-
AdvertiseAddress address = new AdvertiseAddress((String) protocolParameters.get(1), (Integer) protocolParameters.get(2));
36+
List<Object> protocolParameters = Arrays.asList(value);
37+
//assert(protocolParameters.size() == 3);
38+
//assert(protocolParameters.get(0).equals(ProtocolNames.TCPROS));
39+
ArrayList params = (ArrayList) protocolParameters.get(0);
40+
AdvertiseAddress address = new AdvertiseAddress((String) params.get(1), (Integer) params.get(2));
3941
return new TcpRosProtocolDescription(address);
4042
}
4143
}

‎src/main/java/org/ros/internal/node/server/RpcServer.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,12 +33,14 @@ public abstract class RpcServer {
3333

3434
public RpcServer(BindAddress bindAddress, AdvertiseAddress advertiseAddress) throws IOException {
3535
this.advertiseAddress = advertiseAddress;
36+
/*
3637
this.advertiseAddress.setPortCallable(new Callable<Integer>() {
3738
@Override
3839
public Integer call() throws Exception {
3940
return server.getPort();
4041
}
4142
});
43+
*/
4244
}
4345

4446

‎src/main/java/org/ros/internal/node/topic/TopicParticipantManager.java‎

Lines changed: 23 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
import org.ros.node.topic.Publisher;
2323
import org.ros.node.topic.Subscriber;
2424

25+
import java.util.ArrayList;
2526
import java.util.Collection;
2627
import java.util.HashMap;
2728
import java.util.List;
@@ -121,30 +122,40 @@ public void addSubscriberConnection(DefaultSubscriber<?> subscriber, PublisherId
121122
//log.info("sub:"+subscriber+" pub:"+publisherIdentifier);
122123
List<PublisherIdentifier> pubs = subscriberConnections.get(subscriber);
123124
//for(PublisherIdentifier p: pubs) log.info("Pub: "+p);
124-
assert(pubs != null);
125-
if( pubs.contains(publisherIdentifier))
126-
return;
127-
pubs.add(publisherIdentifier);
125+
if( pubs == null ) {
126+
pubs = new ArrayList<PublisherIdentifier>();
127+
pubs.add(publisherIdentifier);
128+
subscriberConnections.put(subscriber, pubs);
129+
} else {
130+
if( pubs.contains(publisherIdentifier))
131+
return;
132+
pubs.add(publisherIdentifier);
133+
}
128134
}
129135

130136
public void removeSubscriberConnection(DefaultSubscriber<?> subscriber, PublisherIdentifier publisherIdentifier) {
131137
List<PublisherIdentifier> pubs = subscriberConnections.get(subscriber);
132-
assert(pubs != null);
133-
pubs.remove(publisherIdentifier);
138+
if(pubs != null);
139+
pubs.remove(publisherIdentifier);
134140
}
135141

136142
public void addPublisherConnection(DefaultPublisher<?> publisher, SubscriberIdentifier subscriberIdentifier) {
137143
List<SubscriberIdentifier> subs = publisherConnections.get(publisher);
138-
assert(subs != null);
139-
if( subs.contains(subscriberIdentifier) )
140-
return;
141-
subs.add(subscriberIdentifier);
144+
if( subs == null) {
145+
subs = new ArrayList<SubscriberIdentifier>();
146+
subs.add(subscriberIdentifier);
147+
publisherConnections.put(publisher, subs);
148+
} else {
149+
if( subs.contains(subscriberIdentifier) )
150+
return;
151+
subs.add(subscriberIdentifier);
152+
}
142153
}
143154

144155
public void removePublisherConnection(DefaultPublisher<?> publisher, SubscriberIdentifier subscriberIdentifier) {
145156
List<SubscriberIdentifier> subs = publisherConnections.get(publisher);
146-
assert(subs != null);
147-
subs.remove(subscriberIdentifier);
157+
if(subs != null);
158+
subs.remove(subscriberIdentifier);
148159
}
149160

150161
public Collection<DefaultSubscriber<?>> getSubscribers() {

‎src/main/java/org/ros/internal/transport/tcp/TcpRosServer.java‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,12 +87,15 @@ public void start() {
8787
topicParticipantManager, serviceManager));
8888

8989
outgoingChannel = bootstrap.bind(bindAddress.toInetSocketAddress());
90+
advertiseAddress.setPort(((InetSocketAddress)(outgoingChannel.getLocalAddress())).getPort());
91+
/*
9092
advertiseAddress.setPortCallable(new Callable<Integer>() {
9193
@Override
9294
public Integer call() throws Exception {
9395
return ((InetSocketAddress) outgoingChannel.getLocalAddress()).getPort();
9496
}
9597
});
98+
*/
9699
if (DEBUG) {
97100
log.info("TcpRosServer starting and Bound to: " + bindAddress);
98101
log.info("TcpRosServer starting and Advertising: " + advertiseAddress);

‎src/main/java/org/ros/node/NodeConfiguration.java‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -76,8 +76,10 @@ public class NodeConfiguration {
7676
private MessageFactory serviceResponseMessageFactory;
7777

7878
private BindAddress tcpRosBindAddress;
79+
private AdvertiseAddress tcpRosAdvertiseAddress = null;
7980
private AdvertiseAddressFactory tcpRosAdvertiseAddressFactory;
8081
private BindAddress rpcBindAddress;
82+
private AdvertiseAddress rpcAdvertiseAddress = null;
8183
private AdvertiseAddressFactory rpcAdvertiseAddressFactory;
8284
private ScheduledExecutorService scheduledExecutorService;
8385
private TimeProvider timeProvider;
@@ -433,8 +435,7 @@ public AdvertiseAddressFactory getTcpRosAdvertiseAddressFactory() {
433435
* server
434436
* @return this {@link NodeConfiguration}
435437
*/
436-
public NodeConfiguration setTcpRosAdvertiseAddressFactory(
437-
AdvertiseAddressFactory tcpRosAdvertiseAddressFactory) {
438+
public NodeConfiguration setTcpRosAdvertiseAddressFactory(AdvertiseAddressFactory tcpRosAdvertiseAddressFactory) {
438439
this.tcpRosAdvertiseAddressFactory = tcpRosAdvertiseAddressFactory;
439440
return this;
440441
}
@@ -445,7 +446,9 @@ public NodeConfiguration setTcpRosAdvertiseAddressFactory(
445446
* @return the {@link AdvertiseAddress} for the {@link Node}'s TCPROS server
446447
*/
447448
public AdvertiseAddress getTcpRosAdvertiseAddress() {
448-
return tcpRosAdvertiseAddressFactory.newDefault();
449+
if( tcpRosAdvertiseAddress == null)
450+
tcpRosAdvertiseAddress = tcpRosAdvertiseAddressFactory.newDefault();
451+
return tcpRosAdvertiseAddress;
449452
}
450453

451454
/**
@@ -477,7 +480,9 @@ public NodeConfiguration setRpcBindAddress(BindAddress rpcBindAddress) {
477480
* @return the {@link AdvertiseAddress} for the {@link Node}'s RPC server
478481
*/
479482
public AdvertiseAddress getRpcAdvertiseAddress() {
480-
return rpcAdvertiseAddressFactory.newDefault();
483+
if( rpcAdvertiseAddress == null)
484+
rpcAdvertiseAddress = rpcAdvertiseAddressFactory.newDefault();
485+
return rpcAdvertiseAddress;
481486
}
482487

483488
/**

0 commit comments

Comments
 (0)