Skip to content

Commit 054f8f9

Browse files
rjgriffin42rjgriffin42
committed
deleted the state estimator api, and restored the purpose of the controller api back to what it was
1 parent f38284f commit 054f8f9

File tree

47 files changed

+309
-316
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

47 files changed

+309
-316
lines changed

‎atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,9 @@
1010
import us.ihmc.commons.exception.DefaultExceptionHandler;
1111
import us.ihmc.commons.exception.ExceptionTools;
1212
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
13+
import us.ihmc.communication.HumanoidControllerAPI;
1314
import us.ihmc.communication.PerceptionAPI;
14-
import us.ihmc.communication.StateEstimatorAPI;
15+
import us.ihmc.communication.controllerAPI.ControllerAPI;
1516
import us.ihmc.ros2.ROS2Input;
1617
import us.ihmc.communication.ROS2Tools;
1718
import us.ihmc.communication.producers.VideoDataServerImageCallback;
@@ -34,6 +35,7 @@
3435
import us.ihmc.robotics.robotSide.RobotSide;
3536
import us.ihmc.ros2.ROS2Node;
3637
import us.ihmc.ros2.ROS2PublisherBasics;
38+
import us.ihmc.ros2.ROS2Topic;
3739
import us.ihmc.simulationConstructionSetTools.util.environments.*;
3840
import us.ihmc.simulationconstructionset.*;
3941
import us.ihmc.simulationconstructionset.Robot;
@@ -65,7 +67,9 @@ public class SCSDoorAndCameraSimulator
6567

6668
public SCSDoorAndCameraSimulator(ROS2Node ros2Node, CommonAvatarEnvironmentInterface environment, DRCRobotModel robotModel, boolean startMinimized)
6769
{
68-
robotConfigurationData = new ROS2Input<>(ros2Node, StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
70+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
71+
72+
robotConfigurationData = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
6973

7074
syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node);
7175

@@ -119,8 +123,6 @@ public SCSDoorAndCameraSimulator(ROS2Node ros2Node, CommonAvatarEnvironmentInter
119123
// must create a joint and attach a CameraMount; make it another robot?
120124

121125
// required for timestamp
122-
ROS2Input<RobotConfigurationData> robotConfigurationData = new ROS2Input<>(ros2Node,
123-
StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
124126
ROS2PublisherBasics<VideoPacket> scsCameraPublisher = ros2Node.createPublisher(ROS2Tools.IHMC_ROOT.withTypeName(VideoPacket.class));
125127
CameraConfiguration cameraConfiguration = new CameraConfiguration(videoCameraMountName);
126128
cameraConfiguration.setCameraMount(videoCameraMountName);

‎atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,8 @@
1010
import us.ihmc.commons.exception.DefaultExceptionHandler;
1111
import us.ihmc.commons.exception.ExceptionTools;
1212
import us.ihmc.commons.thread.ThreadTools;
13-
import us.ihmc.communication.StateEstimatorAPI;
13+
import us.ihmc.communication.HumanoidControllerAPI;
14+
import us.ihmc.communication.controllerAPI.ControllerAPI;
1415
import us.ihmc.ros2.ROS2PublisherBasics;
1516
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
1617
import us.ihmc.communication.PerceptionAPI;
@@ -36,6 +37,7 @@
3637
import us.ihmc.robotics.robotDescription.LidarSensorDescription;
3738
import us.ihmc.robotics.robotSide.RobotSide;
3839
import us.ihmc.ros2.ROS2Node;
40+
import us.ihmc.ros2.ROS2Topic;
3941
import us.ihmc.simulationConstructionSetTools.util.environments.*;
4042
import us.ihmc.simulationconstructionset.*;
4143
import us.ihmc.simulationconstructionset.simulatedSensors.LidarMount;
@@ -73,7 +75,8 @@ public SCSLidarAndCameraSimulator(PubSubImplementation pubSubImplementation, Ter
7375
{
7476
ros2Node = ROS2Tools.createROS2Node(pubSubImplementation, "lidar_and_camera");
7577

76-
robotConfigurationData = new ROS2Input<>(ros2Node, StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
78+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
79+
robotConfigurationData = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
7780

7881
syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node);
7982

@@ -113,8 +116,9 @@ public SCSLidarAndCameraSimulator(PubSubImplementation pubSubImplementation, Ter
113116
// must create a joint and attach a CameraMount; make it another robot?
114117

115118
// required for timestamp
119+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
116120
ROS2Input<RobotConfigurationData> robotConfigurationData = new ROS2Input<>(ros2Node,
117-
StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
121+
ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
118122
ROS2PublisherBasics<VideoPacket> scsCameraPublisher = ros2Node.createPublisher(PerceptionAPI.VIDEO);
119123
CameraConfiguration cameraConfiguration = new CameraConfiguration(videoCameraMountName);
120124
cameraConfiguration.setCameraMount(videoCameraMountName);

‎atlas/src/main/java/us/ihmc/atlas/sensors/AtlasSensorSuiteManager.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@
1414
import us.ihmc.communication.HumanoidControllerAPI;
1515
import us.ihmc.communication.PerceptionAPI;
1616
import us.ihmc.communication.ROS2Tools;
17-
import us.ihmc.communication.StateEstimatorAPI;
1817
import us.ihmc.communication.configuration.NetworkParameters;
18+
import us.ihmc.communication.controllerAPI.ControllerAPI;
1919
import us.ihmc.communication.net.ObjectCommunicator;
2020
import us.ihmc.perception.ros1.camera.FisheyeCameraReceiver;
2121
import us.ihmc.perception.ros1.camera.SCSCameraDataReceiver;
@@ -24,6 +24,7 @@
2424
import us.ihmc.robotModels.FullHumanoidRobotModelFactory;
2525
import us.ihmc.ros2.ROS2Node;
2626
import us.ihmc.ros2.ROS2NodeInterface;
27+
import us.ihmc.ros2.ROS2Topic;
2728
import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataBuffer;
2829
import us.ihmc.sensorProcessing.parameters.AvatarRobotCameraParameters;
2930
import us.ihmc.sensorProcessing.parameters.AvatarRobotLidarParameters;
@@ -113,7 +114,8 @@ public void initializeSimulatedSensors(ObjectCommunicator scsSensorsCommunicator
113114
{
114115
if (enableVideoPublisher)
115116
{
116-
ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName),
117+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
118+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
117119
s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData()));
118120

119121
cameraDataReceiver = new SCSCameraDataReceiver(sensorInformation.getCameraParameters(0).getRobotSide(),

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarControllerThread.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
import us.ihmc.commonWalkingControlModules.visualizer.InverseDynamicsMechanismReferenceFrameVisualizer;
2121
import us.ihmc.commons.Conversions;
2222
import us.ihmc.communication.HumanoidControllerAPI;
23+
import us.ihmc.communication.controllerAPI.ControllerAPI;
2324
import us.ihmc.ros2.ROS2PublisherBasics;
2425
import us.ihmc.communication.packets.ControllerCrashLocation;
2526
import us.ihmc.communication.packets.MessageTools;
@@ -34,6 +35,7 @@
3435
import us.ihmc.robotics.sensors.ForceSensorDataHolder;
3536
import us.ihmc.robotics.sensors.ForceSensorDataHolderReadOnly;
3637
import us.ihmc.robotics.time.ExecutionTimer;
38+
import us.ihmc.ros2.ROS2Topic;
3739
import us.ihmc.ros2.RealtimeROS2Node;
3840
import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
3941
import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
@@ -120,9 +122,10 @@ public AvatarControllerThread(String robotName,
120122
contextDataFactory.setSensorDataContext(new SensorDataContext(controllerFullRobotModel));
121123
humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
122124

125+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
123126
if (realtimeROS2Node != null)
124127
{
125-
crashNotificationPublisher = realtimeROS2Node.createPublisher(HumanoidControllerAPI.getTopic(ControllerCrashNotificationPacket.class, robotName));
128+
crashNotificationPublisher = realtimeROS2Node.createPublisher(ControllerAPI.getTopic(controllerOutputTopic, ControllerCrashNotificationPacket.class));
126129
}
127130
else
128131
{

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/ROS2SyncedRobotModel.java‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,14 @@
55
import controller_msgs.msg.dds.RobotConfigurationData;
66
import us.ihmc.avatar.sakeGripper.ROS2SakeHandStatus;
77
import us.ihmc.communication.HumanoidControllerAPI;
8-
import us.ihmc.communication.StateEstimatorAPI;
8+
import us.ihmc.communication.controllerAPI.ControllerAPI;
99
import us.ihmc.robotics.robotSide.RobotSide;
1010
import us.ihmc.robotics.robotSide.SideDependentList;
1111
import us.ihmc.ros2.ROS2Input;
1212
import us.ihmc.robotModels.FullHumanoidRobotModel;
1313
import us.ihmc.robotModels.FullRobotModelUtils;
1414
import us.ihmc.ros2.ROS2NodeInterface;
15+
import us.ihmc.ros2.ROS2Topic;
1516

1617
import java.util.function.Consumer;
1718

@@ -31,8 +32,9 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node
3132
{
3233
super(robotModel, fullRobotModel, robotModel.getHandModels(), robotModel.getSensorInformation());
3334

35+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
3436
robotConfigurationDataInput = new ROS2Input<>(ros2Node,
35-
StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()),
37+
ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
3638
robotConfigurationData,
3739
message ->
3840
{
@@ -42,12 +44,12 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node
4244
robotConfigurationDataInput.addCallback(message -> resetDataReceptionTimer());
4345
capturabilityBasedStatusInput = new ROS2Input<>(ros2Node,
4446
CapturabilityBasedStatus.class,
45-
HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotModel.getSimpleRobotName()));
47+
ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class));
4648

4749
for (RobotSide robotSide : RobotSide.values)
4850
{
4951
handJointAnglePacketInputs.set(robotSide, new ROS2Input<>(ros2Node,
50-
StateEstimatorAPI.getHandJointAnglesTopic(robotModel.getSimpleRobotName()),
52+
ControllerAPI.getTopic(controllerOutputTopic, HandJointAnglePacket.class),
5153
null,
5254
message -> robotSide.toByte() == message.getRobotSide()));
5355
sakeHandStatus.put(robotSide, new ROS2SakeHandStatus(ros2Node, robotModel.getSimpleRobotName(), robotSide));

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/AvatarSimulationFactory.java‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.JoystickBasedSteppingPluginFactory;
2929
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPluginFactory;
3030
import us.ihmc.communication.HumanoidControllerAPI;
31-
import us.ihmc.communication.StateEstimatorAPI;
31+
import us.ihmc.communication.controllerAPI.ControllerAPI;
3232
import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler.TaskOverrunBehavior;
3333
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
3434
import us.ihmc.euclid.transform.RigidBodyTransform;
@@ -50,7 +50,6 @@
5050
import us.ihmc.robotics.physics.CollidableHelper;
5151
import us.ihmc.robotics.physics.MultiBodySystemStateWriter;
5252
import us.ihmc.robotics.physics.RobotCollisionModel;
53-
import us.ihmc.ros2.ROS2Topic;
5453
import us.ihmc.ros2.RealtimeROS2Node;
5554
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputWriter;
5655
import us.ihmc.sensorProcessing.parameters.AvatarRobotLidarParameters;
@@ -274,7 +273,7 @@ private void setupStateEstimationThread()
274273
else
275274
{
276275
pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeROS2Node.get(), robotName);
277-
realtimeROS2Node.get().createSubscription(StateEstimatorAPI.getTopic(StampedPosePacket.class, robotName),
276+
realtimeROS2Node.get().createSubscription(ControllerAPI.getTopic(HumanoidControllerAPI.getInputTopic(robotName), StampedPosePacket.class),
278277
s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData()));
279278
}
280279

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
import us.ihmc.avatar.networkProcessor.modules.ToolboxController;
1313
import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
1414
import us.ihmc.communication.HumanoidControllerAPI;
15-
import us.ihmc.communication.StateEstimatorAPI;
1615
import us.ihmc.communication.ToolboxAPIs;
16+
import us.ihmc.communication.controllerAPI.ControllerAPI;
1717
import us.ihmc.ros2.ROS2NodeInterface;
1818
import us.ihmc.ros2.ROS2Topic;
1919
import us.ihmc.communication.controllerAPI.command.Command;
@@ -45,12 +45,13 @@ public KinematicsPlanningToolboxModule(DRCRobotModel robotModel, boolean startYo
4545
@Override
4646
public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
4747
{
48-
ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
48+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
49+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
4950
{
5051
if (kinematicsPlanningToolboxController != null)
5152
kinematicsPlanningToolboxController.updateRobotConfigurationData(s.takeNextData());
5253
});
53-
ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
54+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
5455
{
5556
if (kinematicsPlanningToolboxController != null)
5657
kinematicsPlanningToolboxController.updateCapturabilityBasedStatus(s.takeNextData());

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,9 @@
88
import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxController.RobotConfigurationDataBasedUpdater;
99
import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
1010
import us.ihmc.communication.HumanoidControllerAPI;
11-
import us.ihmc.communication.StateEstimatorAPI;
1211
import us.ihmc.communication.ToolboxAPIs;
1312
import us.ihmc.communication.controllerAPI.CommandInputManager;
13+
import us.ihmc.communication.controllerAPI.ControllerAPI;
1414
import us.ihmc.communication.controllerAPI.command.Command;
1515
import us.ihmc.euclid.interfaces.Settable;
1616
import us.ihmc.humanoidRobotics.communication.kinematicsToolboxAPI.HumanoidKinematicsToolboxConfigurationCommand;
@@ -97,7 +97,9 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
9797
{
9898
RobotConfigurationData robotConfigurationData = new RobotConfigurationData();
9999

100-
ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
100+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
101+
102+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
101103
{
102104
if (kinematicsToolBoxController != null)
103105
{
@@ -106,9 +108,10 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
106108
}
107109
});
108110

111+
109112
CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus();
110113

111-
ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
114+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
112115
{
113116
if (kinematicsToolBoxController != null)
114117
{
@@ -119,7 +122,7 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
119122

120123
MultiContactBalanceStatus multiContactBalanceStatus = new MultiContactBalanceStatus();
121124

122-
ros2Node.createSubscription(HumanoidControllerAPI.getTopic(MultiContactBalanceStatus.class, robotName), s ->
125+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, MultiContactBalanceStatus.class), s ->
123126
{
124127
if (kinematicsToolBoxController != null)
125128
{

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,11 +21,12 @@
2121
import us.ihmc.commons.Conversions;
2222
import us.ihmc.communication.HumanoidControllerAPI;
2323
import us.ihmc.communication.ROS2Tools;
24-
import us.ihmc.communication.StateEstimatorAPI;
24+
import us.ihmc.communication.controllerAPI.ControllerAPI;
2525
import us.ihmc.communication.packets.Packet;
2626
import us.ihmc.idl.serializers.extra.JSONSerializer;
2727
import us.ihmc.log.LogTools;
2828
import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
29+
import us.ihmc.ros2.ROS2Topic;
2930
import us.ihmc.ros2.RealtimeROS2Node;
3031
import us.ihmc.tools.thread.CloseableAndDisposable;
3132

@@ -75,9 +76,10 @@ public KinematicsStreamingToolboxMessageLogger(String robotName, PubSubImplement
7576
ros2Node = ROS2Tools.createRealtimeROS2Node(pubSubImplementation,
7677
"ihmc_" + CaseFormat.UPPER_CAMEL.to(CaseFormat.LOWER_UNDERSCORE, "KinematicsStreamingToolboxMessageLogger"));
7778

78-
ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName),
79+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
80+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
7981
s -> robotConfigurationData.set(s.takeNextData()));
80-
ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName),
82+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class),
8183
s -> capturabilityBasedStatus.set(s.takeNextData()));
8284

8385
ros2Node.createSubscription(KinematicsStreamingToolboxModule.getInputTopic(robotName).withTypeName(ToolboxStateMessage.class),

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@
1818
import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
1919
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WholeBodySetpointParameters;
2020
import us.ihmc.communication.HumanoidControllerAPI;
21-
import us.ihmc.communication.StateEstimatorAPI;
2221
import us.ihmc.communication.ToolboxAPIs;
2322
import us.ihmc.communication.controllerAPI.ControllerAPI;
2423
import us.ihmc.communication.controllerAPI.command.Command;
@@ -118,20 +117,23 @@ private static Map<String, Double> fromStandPrep(DRCRobotModel robotModel)
118117
@Override
119118
public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
120119
{
121-
trajectoryMessagePublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyTrajectoryMessage.class, robotName));
122-
streamingMessagePublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyStreamingMessage.class, robotName));
120+
ROS2Topic<?> controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName);
121+
ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
122+
123+
trajectoryMessagePublisher = ros2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, WholeBodyTrajectoryMessage.class));
124+
streamingMessagePublisher = ros2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, WholeBodyStreamingMessage.class));
123125

124126
RobotConfigurationData robotConfigurationData = new RobotConfigurationData();
125127

126-
ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
128+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
127129
{
128130
s.takeNextData(robotConfigurationData, null);
129131
robotStateUpdater.setRobotConfigurationData(robotConfigurationData);
130132
});
131133

132134
CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus();
133135

134-
ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
136+
ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
135137
{
136138
if (controller != null)
137139
{

0 commit comments

Comments
 (0)