1
1
package us .ihmc .avatar .networkProcessor ;
2
2
3
- import java .io .IOException ;
4
- import java .util .HashMap ;
5
- import java .util .Map .Entry ;
6
-
7
3
import us .ihmc .avatar .drcRobot .DRCRobotModel ;
8
4
import us .ihmc .avatar .networkProcessor .footstepPlanningToolboxModule .FootstepPlanningToolboxModule ;
9
5
import us .ihmc .avatar .networkProcessor .kinematicsToolboxModule .KinematicsToolboxModule ;
16
12
import us .ihmc .communication .PacketRouter ;
17
13
import us .ihmc .communication .configuration .NetworkParameterKeys ;
18
14
import us .ihmc .communication .configuration .NetworkParameters ;
19
- import us .ihmc .communication .net .KryoObjectServer ;
20
15
import us .ihmc .communication .packetCommunicator .PacketCommunicator ;
21
16
import us .ihmc .communication .packets .PacketDestination ;
22
17
import us .ihmc .communication .util .NetworkPorts ;
23
18
import us .ihmc .humanoidBehaviors .IHMCHumanoidBehaviorManager ;
24
19
import us .ihmc .humanoidRobotics .kryo .IHMCCommunicationKryoNetClassList ;
25
20
import us .ihmc .multicastLogDataProtocol .modelLoaders .LogModelProvider ;
26
21
import us .ihmc .robotBehaviors .watson .TextToSpeechNetworkModule ;
27
- import us .ihmc .robotEnvironmentAwareness .communication .REACommunicationKryoNetClassLists ;
28
22
import us .ihmc .robotModels .FullHumanoidRobotModel ;
29
23
import us .ihmc .robotics .robotSide .RobotSide ;
30
24
import us .ihmc .sensorProcessing .parameters .DRCRobotSensorInformation ;
31
25
import us .ihmc .tools .io .printing .PrintTools ;
32
26
27
+ import java .io .IOException ;
28
+ import java .util .HashMap ;
29
+ import java .util .Map .Entry ;
30
+
33
31
public class DRCNetworkProcessor
34
32
{
35
33
private static final IHMCCommunicationKryoNetClassList NET_CLASS_LIST = new IHMCCommunicationKryoNetClassList ();
@@ -56,7 +54,6 @@ public DRCNetworkProcessor(DRCRobotModel robotModel, DRCNetworkModuleParameters
56
54
setupFootstepPlanningToolboxModule (robotModel , params );
57
55
addRobotSpecificModuleCommunicators (params .getRobotSpecificModuleCommunicatorPorts ());
58
56
addTextToSpeechEngine (params );
59
- setupRobotEnvironmentAwarenessModule (params );
60
57
setupHeightQuadTreeToolboxModule (robotModel , params );
61
58
setupLidarScanLogger ();
62
59
setupRemoteObjectDetectionFeedbackEndpoint (params );
@@ -93,7 +90,8 @@ private void setupZeroPoseRobotConfigurationPublisherModule(DRCRobotModel robotM
93
90
{
94
91
if (params .isZeroPoseRobotConfigurationPublisherEnabled ())
95
92
{
96
- PacketCommunicator zeroPosePublisherCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .ZERO_POSE_PRODUCER , NET_CLASS_LIST );
93
+ PacketCommunicator zeroPosePublisherCommunicator = PacketCommunicator
94
+ .createIntraprocessPacketCommunicator (NetworkPorts .ZERO_POSE_PRODUCER , NET_CLASS_LIST );
97
95
packetRouter .attachPacketCommunicator (PacketDestination .ZERO_POSE_PRODUCER , zeroPosePublisherCommunicator );
98
96
try
99
97
{
@@ -118,12 +116,12 @@ private void addRobotSpecificModuleCommunicators(HashMap<NetworkPorts, PacketDes
118
116
packetRouter .attachPacketCommunicator (destinationId , packetCommunicator );
119
117
try
120
118
{
121
- packetCommunicator .connect ();
122
- }
119
+ packetCommunicator .connect ();
120
+ }
123
121
catch (IOException e )
124
122
{
125
- e .printStackTrace ();
126
- }
123
+ e .printStackTrace ();
124
+ }
127
125
printModuleConnectedDebugStatement (destinationId , "addRobotSpecificModuleCommunicators" );
128
126
}
129
127
}
@@ -152,7 +150,8 @@ private void setupRemoteObjectDetectionFeedbackEndpoint(DRCNetworkModuleParamete
152
150
{
153
151
if (params .isRemoteObjectDetectionFeedbackEnabled ())
154
152
{
155
- PacketCommunicator objectDetectionFeedbackCommunicator = PacketCommunicator .createTCPPacketCommunicatorServer (NetworkPorts .VALVE_DETECTOR_FEEDBACK_PORT , NET_CLASS_LIST );
153
+ PacketCommunicator objectDetectionFeedbackCommunicator = PacketCommunicator
154
+ .createTCPPacketCommunicatorServer (NetworkPorts .VALVE_DETECTOR_FEEDBACK_PORT , NET_CLASS_LIST );
156
155
packetRouter .attachPacketCommunicator (PacketDestination .OBJECT_DETECTOR , objectDetectionFeedbackCommunicator );
157
156
try
158
157
{
@@ -175,7 +174,8 @@ private void setupKinematicsToolboxModule(DRCRobotModel robotModel, DRCNetworkMo
175
174
176
175
new KinematicsToolboxModule (robotModel , params .isKinematicsToolboxVisualizerEnabled ());
177
176
178
- PacketCommunicator kinematicsToolboxCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .KINEMATICS_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
177
+ PacketCommunicator kinematicsToolboxCommunicator = PacketCommunicator
178
+ .createIntraprocessPacketCommunicator (NetworkPorts .KINEMATICS_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
179
179
packetRouter .attachPacketCommunicator (PacketDestination .KINEMATICS_TOOLBOX_MODULE , kinematicsToolboxCommunicator );
180
180
kinematicsToolboxCommunicator .connect ();
181
181
@@ -192,7 +192,8 @@ private void setupFootstepPlanningToolboxModule(DRCRobotModel robotModel, DRCNet
192
192
193
193
new FootstepPlanningToolboxModule (robotModel , fullRobotModel , null , params .isFootstepPlanningToolboxVisualizerEnabled ());
194
194
195
- PacketCommunicator footstepPlanningToolboxCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .FOOTSTEP_PLANNING_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
195
+ PacketCommunicator footstepPlanningToolboxCommunicator = PacketCommunicator
196
+ .createIntraprocessPacketCommunicator (NetworkPorts .FOOTSTEP_PLANNING_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
196
197
packetRouter .attachPacketCommunicator (PacketDestination .FOOTSTEP_PLANNING_TOOLBOX_MODULE , footstepPlanningToolboxCommunicator );
197
198
footstepPlanningToolboxCommunicator .connect ();
198
199
@@ -206,7 +207,8 @@ private void setupMultisenseManualTestModule(DRCRobotModel robotModel, DRCNetwor
206
207
{
207
208
new MultisenseMocapManualCalibrationTestModule (robotModel , params .getRosUri ());
208
209
209
- PacketCommunicator multisenseModuleCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .MULTISENSE_MOCAP_MANUAL_CALIBRATION_TEST_MODULE , NET_CLASS_LIST );
210
+ PacketCommunicator multisenseModuleCommunicator = PacketCommunicator
211
+ .createIntraprocessPacketCommunicator (NetworkPorts .MULTISENSE_MOCAP_MANUAL_CALIBRATION_TEST_MODULE , NET_CLASS_LIST );
210
212
packetRouter .attachPacketCommunicator (PacketDestination .MULTISENSE_TEST_MODULE , multisenseModuleCommunicator );
211
213
try
212
214
{
@@ -222,26 +224,26 @@ private void setupMultisenseManualTestModule(DRCRobotModel robotModel, DRCNetwor
222
224
}
223
225
}
224
226
225
- private void setupMocapModule (DRCNetworkModuleParameters params ) throws IOException
227
+ private void setupMocapModule (DRCNetworkModuleParameters params ) throws IOException
226
228
{
227
- if (params .isMocapModuleEnabled ())
228
- {
229
- // new MocapDetectedObjectModule(null, null, null);
229
+ if (params .isMocapModuleEnabled ())
230
+ {
231
+ // new MocapDetectedObjectModule(null, null, null);
230
232
231
- PacketCommunicator mocapModuleCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .MOCAP_MODULE , NET_CLASS_LIST );
232
- packetRouter .attachPacketCommunicator (PacketDestination .MOCAP_MODULE , mocapModuleCommunicator );
233
- mocapModuleCommunicator .connect ();
233
+ PacketCommunicator mocapModuleCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .MOCAP_MODULE , NET_CLASS_LIST );
234
+ packetRouter .attachPacketCommunicator (PacketDestination .MOCAP_MODULE , mocapModuleCommunicator );
235
+ mocapModuleCommunicator .connect ();
234
236
235
- String methodName = "setupMocapModule" ;
236
- printModuleConnectedDebugStatement (PacketDestination .MOCAP_MODULE , methodName );
237
- }
237
+ String methodName = "setupMocapModule" ;
238
+ printModuleConnectedDebugStatement (PacketDestination .MOCAP_MODULE , methodName );
239
+ }
238
240
}
239
241
240
242
private void setupHandModules (DRCRobotModel robotModel , DRCNetworkModuleParameters params ) throws IOException
241
243
{
242
244
if (params .isHandModuleEnabled ())
243
245
{
244
- for (RobotSide robotSide : RobotSide .values )
246
+ for (RobotSide robotSide : RobotSide .values )
245
247
{
246
248
NetworkPorts port = robotSide == RobotSide .LEFT ? NetworkPorts .LEFT_HAND_PORT : NetworkPorts .RIGHT_HAND_PORT ;
247
249
PacketCommunicator handModuleCommunicator = PacketCommunicator .createTCPPacketCommunicatorServer (port , NET_CLASS_LIST );
@@ -264,18 +266,20 @@ private void setupBehaviorModule(DRCRobotModel robotModel, DRCNetworkModuleParam
264
266
265
267
if (params .isAutomaticDiagnosticEnabled ())
266
268
{
267
- IHMCHumanoidBehaviorManager .createBehaviorModuleForAutomaticDiagnostic (robotModel , logModelProvider , params .isBehaviorVisualizerEnabled (), sensorInformation , params .getTimeToWaitBeforeStartingDiagnostics ());
269
+ IHMCHumanoidBehaviorManager
270
+ .createBehaviorModuleForAutomaticDiagnostic (robotModel , logModelProvider , params .isBehaviorVisualizerEnabled (), sensorInformation ,
271
+ params .getTimeToWaitBeforeStartingDiagnostics ());
268
272
}
269
273
else
270
274
{
271
275
new IHMCHumanoidBehaviorManager (robotModel , logModelProvider , params .isBehaviorVisualizerEnabled (), sensorInformation );
272
276
}
273
277
274
- PacketCommunicator behaviorModuleCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .BEHAVIOUR_MODULE_PORT , NET_CLASS_LIST );
278
+ PacketCommunicator behaviorModuleCommunicator = PacketCommunicator
279
+ .createIntraprocessPacketCommunicator (NetworkPorts .BEHAVIOUR_MODULE_PORT , NET_CLASS_LIST );
275
280
packetRouter .attachPacketCommunicator (PacketDestination .BEHAVIOR_MODULE , behaviorModuleCommunicator );
276
281
behaviorModuleCommunicator .connect ();
277
282
278
-
279
283
String methodName = "setupBehaviorModule " ;
280
284
printModuleConnectedDebugStatement (PacketDestination .BEHAVIOR_MODULE , methodName );
281
285
}
@@ -311,11 +315,11 @@ private void setupSensorModule(DRCRobotModel robotModel, DRCNetworkModuleParamet
311
315
}
312
316
sensorSuiteManager .connect ();
313
317
314
- PacketCommunicator sensorSuiteManagerCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .SENSOR_MANAGER , NET_CLASS_LIST );
318
+ PacketCommunicator sensorSuiteManagerCommunicator = PacketCommunicator
319
+ .createIntraprocessPacketCommunicator (NetworkPorts .SENSOR_MANAGER , NET_CLASS_LIST );
315
320
packetRouter .attachPacketCommunicator (PacketDestination .SENSOR_MANAGER , sensorSuiteManagerCommunicator );
316
321
sensorSuiteManagerCommunicator .connect ();
317
322
318
-
319
323
String methodName = "setupSensorModule " ;
320
324
printModuleConnectedDebugStatement (PacketDestination .SENSOR_MANAGER , methodName );
321
325
}
@@ -337,7 +341,7 @@ private void setupUiModule(DRCNetworkModuleParameters params) throws IOException
337
341
338
342
private void setupROSAPIModule (DRCNetworkModuleParameters params ) throws IOException
339
343
{
340
- if (params .isROSAPICommunicatorEnabled ())
344
+ if (params .isROSAPICommunicatorEnabled ())
341
345
{
342
346
PacketCommunicator rosAPICommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .ROS_API_COMMUNICATOR , NET_CLASS_LIST );
343
347
packetRouter .attachPacketCommunicator (PacketDestination .ROS_API , rosAPICommunicator );
@@ -352,15 +356,17 @@ private void setupControllerCommunicator(DRCNetworkModuleParameters params) thro
352
356
if (params .isControllerCommunicatorEnabled ())
353
357
{
354
358
PacketCommunicator controllerPacketCommunicator ;
355
- if (params .isLocalControllerCommunicatorEnabled ())
359
+ if (params .isLocalControllerCommunicatorEnabled ())
356
360
{
357
361
PrintTools .info (this , "Connecting to controller using intra process communication" );
358
362
controllerPacketCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .CONTROLLER_PORT , NET_CLASS_LIST );
359
363
}
360
364
else
361
365
{
362
366
System .out .println ("Connecting to controller using TCP on " + NetworkParameters .getHost (NetworkParameterKeys .robotController ));
363
- controllerPacketCommunicator = PacketCommunicator .createTCPPacketCommunicatorClient (NetworkParameters .getHost (NetworkParameterKeys .robotController ), NetworkPorts .CONTROLLER_PORT , NET_CLASS_LIST );
367
+ controllerPacketCommunicator = PacketCommunicator
368
+ .createTCPPacketCommunicatorClient (NetworkParameters .getHost (NetworkParameterKeys .robotController ), NetworkPorts .CONTROLLER_PORT ,
369
+ NET_CLASS_LIST );
364
370
}
365
371
366
372
packetRouter .attachPacketCommunicator (PacketDestination .CONTROLLER , controllerPacketCommunicator );
@@ -371,25 +377,14 @@ private void setupControllerCommunicator(DRCNetworkModuleParameters params) thro
371
377
}
372
378
}
373
379
374
- private void setupRobotEnvironmentAwarenessModule (DRCNetworkModuleParameters parameters ) throws IOException
375
- {
376
- if (parameters .isRobotEnvironmentAwerenessModuleEnabled ())
377
- {
378
- KryoObjectServer server = new KryoObjectServer (NetworkPorts .REA_MODULE_PORT .getPort (), REACommunicationKryoNetClassLists .getPublicNetClassList ());
379
- server .throwExceptionForUnregisteredPackets (false );
380
- PacketCommunicator reaCommunicator = PacketCommunicator .createCustomPacketCommunicator (server , REACommunicationKryoNetClassLists .getPublicNetClassList ());
381
- packetRouter .attachPacketCommunicator (PacketDestination .REA_MODULE , reaCommunicator );
382
- reaCommunicator .connect ();
383
- }
384
- }
385
-
386
380
private void setupHeightQuadTreeToolboxModule (DRCRobotModel robotModel , DRCNetworkModuleParameters params ) throws IOException
387
381
{
388
382
if (params .isHeightQuadTreeToolboxEnabled ())
389
383
{
390
384
new HeightQuadTreeToolboxModule (robotModel .createFullRobotModel (), robotModel .getLogModelProvider ());
391
385
392
- PacketCommunicator heightQuadTreeToolboxCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator (NetworkPorts .HEIGHT_QUADTREE_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
386
+ PacketCommunicator heightQuadTreeToolboxCommunicator = PacketCommunicator
387
+ .createIntraprocessPacketCommunicator (NetworkPorts .HEIGHT_QUADTREE_TOOLBOX_MODULE_PORT , NET_CLASS_LIST );
393
388
packetRouter .attachPacketCommunicator (PacketDestination .HEIGHT_QUADTREE_TOOLBOX_MODULE , heightQuadTreeToolboxCommunicator );
394
389
heightQuadTreeToolboxCommunicator .connect ();
395
390
@@ -400,7 +395,8 @@ private void setupHeightQuadTreeToolboxModule(DRCRobotModel robotModel, DRCNetwo
400
395
401
396
private void setupLidarScanLogger () throws IOException
402
397
{
403
- PacketCommunicator lidarScanLoggerCommunicator = PacketCommunicator .createTCPPacketCommunicatorServer (NetworkPorts .LIDAR_SCAN_LOGGER_PORT , NET_CLASS_LIST );
398
+ PacketCommunicator lidarScanLoggerCommunicator = PacketCommunicator
399
+ .createTCPPacketCommunicatorServer (NetworkPorts .LIDAR_SCAN_LOGGER_PORT , NET_CLASS_LIST );
404
400
packetRouter .attachPacketCommunicator (PacketDestination .LIDAR_SCAN_LOGGER , lidarScanLoggerCommunicator );
405
401
lidarScanLoggerCommunicator .connect ();
406
402
0 commit comments