Skip to content

Commit f38284f

Browse files
TomaszTBTomaszTB
authored
ROS2DemandGraphNode Demand Changed Callback + Tests (#419)
* Added test + demand changed callback
1 parent 3ec6ca0 commit f38284f

File tree

2 files changed

+167
-2
lines changed

2 files changed

+167
-2
lines changed

‎ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2DemandGraphNode.java‎

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,43 @@
44

55
import java.util.ArrayList;
66
import java.util.Arrays;
7+
import java.util.HashSet;
8+
import java.util.Set;
9+
import java.util.concurrent.atomic.AtomicBoolean;
10+
import java.util.function.Consumer;
711

812
public class ROS2DemandGraphNode
913
{
1014
private final ArrayList<ROS2DemandGraphNode> dependents = new ArrayList<>();
1115
private final ROS2HeartbeatMonitor nodeHeartbeatMonitor;
16+
private final Set<Consumer<Boolean>> demandChangedCallbacks = new HashSet<>();
17+
private final AtomicBoolean wasDemanded = new AtomicBoolean(false);
1218

1319
public ROS2DemandGraphNode(ROS2PublishSubscribeAPI ros2, ROS2Topic<Empty> heartbeatTopic)
1420
{
1521
nodeHeartbeatMonitor = new ROS2HeartbeatMonitor(ros2, heartbeatTopic);
22+
nodeHeartbeatMonitor.setAlivenessChangedCallback(this::checkIfDemandChanged);
1623
}
1724

18-
public void addDependents(ROS2DemandGraphNode... dependents)
25+
public void addDependents(ROS2DemandGraphNode... dependentsToAdd)
1926
{
20-
this.dependents.addAll(Arrays.stream(dependents).toList());
27+
dependents.addAll(Arrays.stream(dependentsToAdd).toList());
28+
for (ROS2DemandGraphNode dependent : dependentsToAdd)
29+
dependent.addDemandChangedCallback(this::checkIfDemandChanged);
30+
}
31+
32+
public void addDemandChangedCallback(Consumer<Boolean> demandChangedCallback)
33+
{
34+
demandChangedCallbacks.add(demandChangedCallback);
35+
}
36+
37+
private void checkIfDemandChanged(boolean ignored)
38+
{
39+
boolean isDemanded = isDemanded();
40+
// if demanded status changed, update wasDemanded to current status and trigger callbacks
41+
if (wasDemanded.compareAndSet(!isDemanded, isDemanded))
42+
for (Consumer<Boolean> demandChangedCallback : demandChangedCallbacks)
43+
demandChangedCallback.accept(isDemanded);
2144
}
2245

2346
public boolean isDemanded()

‎ihmc-communication/src/test/java/us/ihmc/communication/ros2/ROS2DemandGraphNodeTest.java‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
package us.ihmc.communication.ros2;
2+
3+
import org.junit.jupiter.api.Test;
4+
import std_msgs.msg.dds.Empty;
5+
import us.ihmc.commons.thread.ThreadTools;
6+
import us.ihmc.commons.thread.TypedNotification;
7+
import us.ihmc.communication.ROS2Tools;
8+
import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
9+
import us.ihmc.ros2.ROS2Node;
10+
import us.ihmc.ros2.ROS2Topic;
11+
12+
import static org.junit.jupiter.api.Assertions.*;
13+
14+
public class ROS2DemandGraphNodeTest
15+
{
16+
private static final double SLEEP_DURATION = 1.25 * ROS2HeartbeatMonitor.HEARTBEAT_EXPIRATION;
17+
18+
@Test
19+
public void testIsDemanded()
20+
{
21+
ROS2Node ros2Node = ROS2Tools.createROS2Node(PubSubImplementation.FAST_RTPS, "demand_graph_test_is_demanded");
22+
ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
23+
ROS2Topic<Empty> testTopic = ROS2Tools.IHMC_ROOT.withSuffix("demand_graph_test_is_demanded").withType(Empty.class);
24+
25+
ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic);
26+
ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic);
27+
28+
assertFalse(testNode.isDemanded());
29+
30+
testHeartbeat.setAlive(true);
31+
ThreadTools.sleepSeconds(SLEEP_DURATION);
32+
assertTrue(testNode.isDemanded());
33+
34+
testHeartbeat.setAlive(false);
35+
ThreadTools.sleepSeconds(SLEEP_DURATION);
36+
assertFalse(testNode.isDemanded());
37+
38+
testHeartbeat.destroy();
39+
testNode.destroy();
40+
ros2Node.destroy();
41+
}
42+
43+
@Test
44+
public void testDependantDemand()
45+
{
46+
ROS2Node ros2Node = ROS2Tools.createROS2Node(PubSubImplementation.FAST_RTPS, "demand_graph_test_dependant");
47+
ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
48+
ROS2Topic<Empty> testTopic = ROS2Tools.IHMC_ROOT.withSuffix("demand_graph_test_dependant").withType(Empty.class);
49+
ROS2Topic<Empty> dependantTopic = testTopic.withPrefix("dependant");
50+
51+
ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic);
52+
ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic);
53+
ROS2Heartbeat dependantHeartbeat = new ROS2Heartbeat(ros2Node, dependantTopic);
54+
ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Helper, dependantTopic);
55+
testNode.addDependents(dependantNode);
56+
57+
// No heartbeat is on -> nothing should be demanded
58+
assertFalse(testNode.isDemanded());
59+
assertFalse(dependantNode.isDemanded());
60+
61+
// Dependant heartbeat is on -> both nodes should be demanded
62+
dependantHeartbeat.setAlive(true);
63+
ThreadTools.sleepSeconds(SLEEP_DURATION);
64+
assertTrue(dependantNode.isDemanded());
65+
assertTrue(testNode.isDemanded());
66+
67+
// Test heartbeat is on -> only test node should be demanded
68+
dependantHeartbeat.setAlive(false);
69+
testHeartbeat.setAlive(true);
70+
ThreadTools.sleepSeconds(SLEEP_DURATION);
71+
assertFalse(dependantNode.isDemanded());
72+
assertTrue(testNode.isDemanded());
73+
74+
// Neither heartbeat is on -> nothing should be demanded
75+
testHeartbeat.setAlive(false);
76+
ThreadTools.sleepSeconds(SLEEP_DURATION);
77+
assertFalse(dependantNode.isDemanded());
78+
assertFalse(testNode.isDemanded());
79+
80+
testNode.destroy();
81+
testHeartbeat.destroy();
82+
dependantNode.destroy();
83+
dependantHeartbeat.destroy();
84+
ros2Node.destroy();
85+
}
86+
87+
@Test
88+
public void testDemandChangedCallback()
89+
{
90+
ROS2Node ros2Node = ROS2Tools.createROS2Node(PubSubImplementation.FAST_RTPS, "demand_graph_test_dependant");
91+
ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
92+
ROS2Topic<Empty> testTopic = ROS2Tools.IHMC_ROOT.withSuffix("demand_graph_test_dependant").withType(Empty.class);
93+
ROS2Topic<Empty> dependantTopic = testTopic.withPrefix("dependant");
94+
95+
ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic);
96+
ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic);
97+
ROS2Heartbeat dependantHeartbeat = new ROS2Heartbeat(ros2Node, dependantTopic);
98+
ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Helper, dependantTopic);
99+
testNode.addDependents(dependantNode);
100+
101+
TypedNotification<Boolean> callbackNotification = new TypedNotification<>();
102+
testNode.addDemandChangedCallback(callbackNotification::set);
103+
104+
// Demand dependant -> callback should trigger with isDemanded = true
105+
dependantHeartbeat.setAlive(true);
106+
assertTrue(callbackNotification.blockingPoll());
107+
108+
// Demand test node -> callback should NOT trigger as dependant is already demanded
109+
testHeartbeat.setAlive(true);
110+
ThreadTools.sleepSeconds(SLEEP_DURATION);
111+
assertFalse(callbackNotification.poll());
112+
113+
// Un-demand dependant -> callback should NOT trigger as test node is still demanded
114+
dependantHeartbeat.setAlive(false);
115+
ThreadTools.sleepSeconds(ROS2HeartbeatMonitor.HEARTBEAT_EXPIRATION);
116+
assertFalse(callbackNotification.poll());
117+
118+
// Un-demand test node -> callback should trigger with isDemanded = false;
119+
testHeartbeat.setAlive(false);
120+
assertFalse(callbackNotification.blockingPoll());
121+
122+
// Demand both at same time -> callback should trigger only once
123+
dependantHeartbeat.setAlive(true);
124+
testHeartbeat.setAlive(true);
125+
assertTrue(callbackNotification.blockingPoll());
126+
ThreadTools.sleepSeconds(SLEEP_DURATION);
127+
assertFalse(callbackNotification.poll());
128+
129+
// Un-demand both at same time -> callback should trigger only once
130+
dependantHeartbeat.setAlive(false);
131+
testHeartbeat.setAlive(false);
132+
assertFalse(callbackNotification.blockingPoll());
133+
ThreadTools.sleepSeconds(SLEEP_DURATION);
134+
assertFalse(callbackNotification.poll());
135+
136+
dependantNode.destroy();
137+
dependantHeartbeat.destroy();
138+
testNode.destroy();
139+
testHeartbeat.destroy();
140+
ros2Node.destroy();
141+
}
142+
}

0 commit comments

Comments
 (0)