deleted the state estimator api, and restored the purpose of the cont…

…roller api back to what it was
ihmcrobotics · Oct 21, 2024 · 054f8f9 · 054f8f9
1 parent f38284f
commit 054f8f9
Show file tree

Hide file tree

Showing 47 changed files with 309 additions and 316 deletions.
diff --git a/.../src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java b/.../src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java
@@ -10,8 +10,9 @@
 import us.ihmc.commons.exception.DefaultExceptionHandler;
 import us.ihmc.commons.exception.ExceptionTools;
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
+import us.ihmc.communication.HumanoidControllerAPI;
 import us.ihmc.communication.PerceptionAPI;
-import us.ihmc.communication.StateEstimatorAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.ros2.ROS2Input;
 import us.ihmc.communication.ROS2Tools;
 import us.ihmc.communication.producers.VideoDataServerImageCallback;
@@ -34,6 +35,7 @@
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2PublisherBasics;
+import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.simulationConstructionSetTools.util.environments.*;
 import us.ihmc.simulationconstructionset.*;
 import us.ihmc.simulationconstructionset.Robot;
@@ -65,7 +67,9 @@ public class SCSDoorAndCameraSimulator
 
    public SCSDoorAndCameraSimulator(ROS2Node ros2Node, CommonAvatarEnvironmentInterface environment, DRCRobotModel robotModel, boolean startMinimized)
    {
-      robotConfigurationData = new ROS2Input<>(ros2Node, StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
+
+      robotConfigurationData = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
 
       syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node);
 
@@ -119,8 +123,6 @@ public SCSDoorAndCameraSimulator(ROS2Node ros2Node, CommonAvatarEnvironmentInter
       // must create a joint and attach a CameraMount; make it another robot?
 
       // required for timestamp
-      ROS2Input<RobotConfigurationData> robotConfigurationData = new ROS2Input<>(ros2Node,
-                                                                                 StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
       ROS2PublisherBasics<VideoPacket> scsCameraPublisher = ros2Node.createPublisher(ROS2Tools.IHMC_ROOT.withTypeName(VideoPacket.class));
       CameraConfiguration cameraConfiguration = new CameraConfiguration(videoCameraMountName);
       cameraConfiguration.setCameraMount(videoCameraMountName);

diff --git a/...src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java b/...src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java
@@ -10,7 +10,8 @@
 import us.ihmc.commons.exception.DefaultExceptionHandler;
 import us.ihmc.commons.exception.ExceptionTools;
 import us.ihmc.commons.thread.ThreadTools;
-import us.ihmc.communication.StateEstimatorAPI;
+import us.ihmc.communication.HumanoidControllerAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
 import us.ihmc.communication.PerceptionAPI;
@@ -36,6 +37,7 @@
 import us.ihmc.robotics.robotDescription.LidarSensorDescription;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.ros2.ROS2Node;
+import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.simulationConstructionSetTools.util.environments.*;
 import us.ihmc.simulationconstructionset.*;
 import us.ihmc.simulationconstructionset.simulatedSensors.LidarMount;
@@ -73,7 +75,8 @@ public SCSLidarAndCameraSimulator(PubSubImplementation pubSubImplementation, Ter
    {
       ros2Node = ROS2Tools.createROS2Node(pubSubImplementation, "lidar_and_camera");
 
-      robotConfigurationData = new ROS2Input<>(ros2Node, StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
+      robotConfigurationData = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
 
       syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node);
 
@@ -113,8 +116,9 @@ public SCSLidarAndCameraSimulator(PubSubImplementation pubSubImplementation, Ter
       // must create a joint and attach a CameraMount; make it another robot?
 
       // required for timestamp
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
       ROS2Input<RobotConfigurationData> robotConfigurationData = new ROS2Input<>(ros2Node,
-                                                                                 StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()));
+                                                                                 ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class));
       ROS2PublisherBasics<VideoPacket> scsCameraPublisher = ros2Node.createPublisher(PerceptionAPI.VIDEO);
       CameraConfiguration cameraConfiguration = new CameraConfiguration(videoCameraMountName);
       cameraConfiguration.setCameraMount(videoCameraMountName);

diff --git a/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasSensorSuiteManager.java b/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasSensorSuiteManager.java
@@ -14,8 +14,8 @@
 import us.ihmc.communication.HumanoidControllerAPI;
 import us.ihmc.communication.PerceptionAPI;
 import us.ihmc.communication.ROS2Tools;
-import us.ihmc.communication.StateEstimatorAPI;
 import us.ihmc.communication.configuration.NetworkParameters;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.communication.net.ObjectCommunicator;
 import us.ihmc.perception.ros1.camera.FisheyeCameraReceiver;
 import us.ihmc.perception.ros1.camera.SCSCameraDataReceiver;
@@ -24,6 +24,7 @@
 import us.ihmc.robotModels.FullHumanoidRobotModelFactory;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2NodeInterface;
+import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataBuffer;
 import us.ihmc.sensorProcessing.parameters.AvatarRobotCameraParameters;
 import us.ihmc.sensorProcessing.parameters.AvatarRobotLidarParameters;
@@ -113,7 +114,8 @@ public void initializeSimulatedSensors(ObjectCommunicator scsSensorsCommunicator
    {
       if (enableVideoPublisher)
       {
-         ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName),
+         ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
+         ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
                                      s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData()));
 
          cameraDataReceiver = new SCSCameraDataReceiver(sensorInformation.getCameraParameters(0).getRobotSide(),

diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarControllerThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarControllerThread.java
@@ -20,6 +20,7 @@
 import us.ihmc.commonWalkingControlModules.visualizer.InverseDynamicsMechanismReferenceFrameVisualizer;
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.HumanoidControllerAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.communication.packets.ControllerCrashLocation;
 import us.ihmc.communication.packets.MessageTools;
@@ -34,6 +35,7 @@
 import us.ihmc.robotics.sensors.ForceSensorDataHolder;
 import us.ihmc.robotics.sensors.ForceSensorDataHolderReadOnly;
 import us.ihmc.robotics.time.ExecutionTimer;
+import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
@@ -120,9 +122,10 @@ public AvatarControllerThread(String robotName,
       contextDataFactory.setSensorDataContext(new SensorDataContext(controllerFullRobotModel));
       humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
 
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
       if (realtimeROS2Node != null)
       {
-         crashNotificationPublisher = realtimeROS2Node.createPublisher(HumanoidControllerAPI.getTopic(ControllerCrashNotificationPacket.class, robotName));
+         crashNotificationPublisher = realtimeROS2Node.createPublisher(ControllerAPI.getTopic(controllerOutputTopic, ControllerCrashNotificationPacket.class));
       }
       else
       {

diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/ROS2SyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/ROS2SyncedRobotModel.java
@@ -5,13 +5,14 @@
 import controller_msgs.msg.dds.RobotConfigurationData;
 import us.ihmc.avatar.sakeGripper.ROS2SakeHandStatus;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.communication.StateEstimatorAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.ros2.ROS2Input;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
 import us.ihmc.robotModels.FullRobotModelUtils;
 import us.ihmc.ros2.ROS2NodeInterface;
+import us.ihmc.ros2.ROS2Topic;
 
 import java.util.function.Consumer;
 
@@ -31,8 +32,9 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node
    {
       super(robotModel, fullRobotModel, robotModel.getHandModels(), robotModel.getSensorInformation());
 
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName());
       robotConfigurationDataInput = new ROS2Input<>(ros2Node,
-                                                    StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()),
+                                                    ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
                                                     robotConfigurationData,
                                                     message ->
                                                     {
@@ -42,12 +44,12 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node
       robotConfigurationDataInput.addCallback(message -> resetDataReceptionTimer());
       capturabilityBasedStatusInput = new ROS2Input<>(ros2Node,
                                                       CapturabilityBasedStatus.class,
-                                                      HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotModel.getSimpleRobotName()));
+                                                      ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class));
 
       for (RobotSide robotSide : RobotSide.values)
       {
          handJointAnglePacketInputs.set(robotSide, new ROS2Input<>(ros2Node,
-                                                                   StateEstimatorAPI.getHandJointAnglesTopic(robotModel.getSimpleRobotName()),
+                                                                   ControllerAPI.getTopic(controllerOutputTopic, HandJointAnglePacket.class),
                                                                    null,
                                                                    message -> robotSide.toByte() == message.getRobotSide()));
          sakeHandStatus.put(robotSide, new ROS2SakeHandStatus(ros2Node, robotModel.getSimpleRobotName(), robotSide));

diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/AvatarSimulationFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/AvatarSimulationFactory.java
@@ -28,7 +28,7 @@
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.JoystickBasedSteppingPluginFactory;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPluginFactory;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.communication.StateEstimatorAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler.TaskOverrunBehavior;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.transform.RigidBodyTransform;
@@ -50,7 +50,6 @@
 import us.ihmc.robotics.physics.CollidableHelper;
 import us.ihmc.robotics.physics.MultiBodySystemStateWriter;
 import us.ihmc.robotics.physics.RobotCollisionModel;
-import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.sensorProcessing.outputData.JointDesiredOutputWriter;
 import us.ihmc.sensorProcessing.parameters.AvatarRobotLidarParameters;
@@ -274,7 +273,7 @@ private void setupStateEstimationThread()
       else
       {
          pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeROS2Node.get(), robotName);
-         realtimeROS2Node.get().createSubscription(StateEstimatorAPI.getTopic(StampedPosePacket.class, robotName),
+         realtimeROS2Node.get().createSubscription(ControllerAPI.getTopic(HumanoidControllerAPI.getInputTopic(robotName), StampedPosePacket.class),
                                                    s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData()));
       }
 

diff --git a/...tar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java b/...tar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java
@@ -12,8 +12,8 @@
 import us.ihmc.avatar.networkProcessor.modules.ToolboxController;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.communication.StateEstimatorAPI;
 import us.ihmc.communication.ToolboxAPIs;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.ros2.ROS2NodeInterface;
 import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.communication.controllerAPI.command.Command;
@@ -45,12 +45,13 @@ public KinematicsPlanningToolboxModule(DRCRobotModel robotModel, boolean startYo
    @Override
    public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
    {
-      ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
       {
          if (kinematicsPlanningToolboxController != null)
             kinematicsPlanningToolboxController.updateRobotConfigurationData(s.takeNextData());
       });
-      ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
       {
          if (kinematicsPlanningToolboxController != null)
             kinematicsPlanningToolboxController.updateCapturabilityBasedStatus(s.takeNextData());

diff --git a/...java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java b/...java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java
@@ -8,9 +8,9 @@
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxController.RobotConfigurationDataBasedUpdater;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.communication.StateEstimatorAPI;
 import us.ihmc.communication.ToolboxAPIs;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.communication.controllerAPI.command.Command;
 import us.ihmc.euclid.interfaces.Settable;
 import us.ihmc.humanoidRobotics.communication.kinematicsToolboxAPI.HumanoidKinematicsToolboxConfigurationCommand;
@@ -97,7 +97,9 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
    {
       RobotConfigurationData robotConfigurationData = new RobotConfigurationData();
 
-      ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
+
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
       {
          if (kinematicsToolBoxController != null)
          {
@@ -106,9 +108,10 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
          }
       });
 
+
       CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus();
 
-      ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
       {
          if (kinematicsToolBoxController != null)
          {
@@ -119,7 +122,7 @@ public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
 
       MultiContactBalanceStatus multiContactBalanceStatus = new MultiContactBalanceStatus();
 
-      ros2Node.createSubscription(HumanoidControllerAPI.getTopic(MultiContactBalanceStatus.class, robotName), s ->
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, MultiContactBalanceStatus.class), s ->
       {
          if (kinematicsToolBoxController != null)
          {

diff --git a/...kProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java b/...kProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java
@@ -21,11 +21,12 @@
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.HumanoidControllerAPI;
 import us.ihmc.communication.ROS2Tools;
-import us.ihmc.communication.StateEstimatorAPI;
+import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.communication.packets.Packet;
 import us.ihmc.idl.serializers.extra.JSONSerializer;
 import us.ihmc.log.LogTools;
 import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
+import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.tools.thread.CloseableAndDisposable;
 
@@ -75,9 +76,10 @@ public KinematicsStreamingToolboxMessageLogger(String robotName, PubSubImplement
       ros2Node = ROS2Tools.createRealtimeROS2Node(pubSubImplementation,
                                                   "ihmc_" + CaseFormat.UPPER_CAMEL.to(CaseFormat.LOWER_UNDERSCORE, "KinematicsStreamingToolboxMessageLogger"));
 
-      ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName),
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class),
                                   s -> robotConfigurationData.set(s.takeNextData()));
-      ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName),
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class),
                                   s -> capturabilityBasedStatus.set(s.takeNextData()));
 
       ros2Node.createSubscription(KinematicsStreamingToolboxModule.getInputTopic(robotName).withTypeName(ToolboxStateMessage.class),

diff --git a/.../networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java b/.../networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java
@@ -18,7 +18,6 @@
 import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WholeBodySetpointParameters;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.communication.StateEstimatorAPI;
 import us.ihmc.communication.ToolboxAPIs;
 import us.ihmc.communication.controllerAPI.ControllerAPI;
 import us.ihmc.communication.controllerAPI.command.Command;
@@ -118,20 +117,23 @@ private static Map<String, Double> fromStandPrep(DRCRobotModel robotModel)
    @Override
    public void registerExtraPuSubs(ROS2NodeInterface ros2Node)
    {
-      trajectoryMessagePublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyTrajectoryMessage.class, robotName));
-      streamingMessagePublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyStreamingMessage.class, robotName));
+      ROS2Topic<?> controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName);
+      ROS2Topic<?> controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
+
+      trajectoryMessagePublisher = ros2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, WholeBodyTrajectoryMessage.class));
+      streamingMessagePublisher = ros2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, WholeBodyStreamingMessage.class));
 
       RobotConfigurationData robotConfigurationData = new RobotConfigurationData();
 
-      ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), s ->
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s ->
       {
          s.takeNextData(robotConfigurationData, null);
          robotStateUpdater.setRobotConfigurationData(robotConfigurationData);
       });
 
       CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus();
 
-      ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), s ->
+      ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), s ->
       {
          if (controller != null)
          {