Skip to content
This repository was archived by the owner on Aug 11, 2023. It is now read-only.

Commit 13bcbee

Browse files
author
Juan Ignacio Ubeira
authored
Merge pull request #264 from drigz/wait-for-master
Wait for master before getting /use_sim_time
2 parents e92b7b7 + 70e445f commit 13bcbee

File tree

1 file changed

+19
-6
lines changed

1 file changed

+19
-6
lines changed

rosjava/src/main/java/org/ros/internal/node/DefaultNode.java

+19-6
Original file line numberDiff line numberDiff line change
@@ -180,42 +180,55 @@ private void start() {
180180
// possible during startup.
181181
registrar.start(slaveServer.toNodeIdentifier());
182182

183-
// During startup, we wait for 1) the RosoutLogger and 2) the TimeProvider.
184-
final CountDownLatch latch = new CountDownLatch(2);
183+
// Wait for the logger to register with the master. This ensures the master is running before
184+
// requesting the use_sim_time parameter.
185+
final CountDownLatch rosoutLatch = new CountDownLatch(1);
185186

186187
log = new RosoutLogger(this);
187188
log.getPublisher().addListener(new DefaultPublisherListener<rosgraph_msgs.Log>() {
188189
@Override
189190
public void onMasterRegistrationSuccess(Publisher<rosgraph_msgs.Log> registrant) {
190-
latch.countDown();
191+
rosoutLatch.countDown();
191192
}
192193
});
193194

195+
try {
196+
rosoutLatch.await();
197+
} catch (InterruptedException e) {
198+
signalOnError(e);
199+
shutdown();
200+
return;
201+
}
202+
194203
boolean useSimTime = false;
195204
try {
196205
useSimTime =
197206
parameterTree.has(Parameters.USE_SIM_TIME)
198207
&& parameterTree.getBoolean(Parameters.USE_SIM_TIME);
199208
} catch (Exception e) {
200209
signalOnError(e);
210+
shutdown();
211+
return;
201212
}
213+
214+
final CountDownLatch timeLatch = new CountDownLatch(1);
202215
if (useSimTime) {
203216
ClockTopicTimeProvider clockTopicTimeProvider = new ClockTopicTimeProvider(this);
204217
clockTopicTimeProvider.getSubscriber().addSubscriberListener(
205218
new DefaultSubscriberListener<rosgraph_msgs.Clock>() {
206219
@Override
207220
public void onMasterRegistrationSuccess(Subscriber<rosgraph_msgs.Clock> subscriber) {
208-
latch.countDown();
221+
timeLatch.countDown();
209222
}
210223
});
211224
timeProvider = clockTopicTimeProvider;
212225
} else {
213226
timeProvider = nodeConfiguration.getTimeProvider();
214-
latch.countDown();
227+
timeLatch.countDown();
215228
}
216229

217230
try {
218-
latch.await();
231+
timeLatch.await();
219232
} catch (InterruptedException e) {
220233
signalOnError(e);
221234
shutdown();

0 commit comments

Comments
 (0)