diff --git a/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java b/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java index 55e30ba63214..301c674813a9 100644 --- a/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSDoorAndCameraSimulator.java +++ b/atlas/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 = new ROS2Input<>(ros2Node, - StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName())); ROS2PublisherBasics scsCameraPublisher = ros2Node.createPublisher(ROS2Tools.IHMC_ROOT.withTypeName(VideoPacket.class)); CameraConfiguration cameraConfiguration = new CameraConfiguration(videoCameraMountName); cameraConfiguration.setCameraMount(videoCameraMountName); diff --git a/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java b/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java index c7936c248255..fbfd243e7caf 100644 --- a/atlas/src/main/java/us/ihmc/atlas/behaviors/scsSensorSimulation/SCSLidarAndCameraSimulator.java +++ b/atlas/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 = new ROS2Input<>(ros2Node, - StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName())); + ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class)); ROS2PublisherBasics 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 index 0d6faf3b5548..f6762518cf68 100644 --- 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 index 1898d1157376..16fda6d4930f 100644 --- 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 index 0864df1fc8e0..a87f206dae59 100644 --- 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 index 75056f097b60..a33c71e4ec1e 100644 --- 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/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java index 236a707b251c..c015eda3bb1f 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsPlanningToolboxModule/KinematicsPlanningToolboxModule.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/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/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java index 76a916e93c26..3374e4769593 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxModule.java +++ b/ihmc-avatar-interfaces/src/main/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/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java index b1c85c5e521f..85f2008b7625 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxMessageLogger.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/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/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java index 851922c882d9..b34e5550ebe6 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/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,12 +117,15 @@ private static Map 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); @@ -131,7 +133,7 @@ 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 (controller != null) { diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/lidarScanPublisher/LidarScanPublisher.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/lidarScanPublisher/LidarScanPublisher.java index 7e3bf48f0727..e57b363b206a 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/lidarScanPublisher/LidarScanPublisher.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/lidarScanPublisher/LidarScanPublisher.java @@ -5,13 +5,15 @@ import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicReference; +import controller_msgs.msg.dds.RobotConfigurationData; import perception_msgs.msg.dds.LidarScanMessage; import perception_msgs.msg.dds.SimulatedLidarScanPacket; import gnu.trove.list.array.TFloatArrayList; import scan_to_cloud.PointCloud2WithSource; import sensor_msgs.PointCloud2; +import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.perception.filters.CollidingScanPointFilter; import us.ihmc.perception.depthData.PointCloudData; import us.ihmc.avatar.networkProcessor.stereoPointCloudPublisher.RangeScanPointFilter; @@ -34,6 +36,7 @@ import us.ihmc.robotics.lidar.LidarScanParameters; import us.ihmc.ros2.ROS2NodeInterface; import us.ihmc.ros2.ROS2QosProfile; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataBuffer; import us.ihmc.tools.thread.ExceptionHandlingThreadScheduler; import us.ihmc.utilities.ros.RosMainNode; @@ -105,7 +108,8 @@ public LidarScanPublisher(String robotName, this.fullRobotModel = fullRobotModel; lidarSensorFrame = sensorFrameFactory.setupSensorFrame(fullRobotModel); - ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + ROS2Topic outputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(outputTopic, RobotConfigurationData.class), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); lidarScanPublisher = ros2Node.createPublisher(PerceptionAPI.MULTISENSE_LIDAR_SCAN); } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/stereoPointCloudPublisher/StereoVisionPointCloudPublisher.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/stereoPointCloudPublisher/StereoVisionPointCloudPublisher.java index f102c72bd074..7d16c5ae4583 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/stereoPointCloudPublisher/StereoVisionPointCloudPublisher.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/stereoPointCloudPublisher/StereoVisionPointCloudPublisher.java @@ -11,6 +11,7 @@ import com.google.common.util.concurrent.AtomicDouble; +import controller_msgs.msg.dds.RobotConfigurationData; import controller_msgs.msg.dds.StereoVisionPointCloudMessage; import controller_msgs.msg.dds.StereoVisionPointCloudMessagePubSubType; import sensor_msgs.PointCloud2; @@ -18,7 +19,8 @@ import us.ihmc.avatar.ros.RobotROSClockCalculator; import us.ihmc.commons.Conversions; 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.euclid.geometry.Pose3D; import us.ihmc.euclid.geometry.interfaces.Pose3DBasics; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -111,7 +113,8 @@ public StereoVisionPointCloudPublisher(String robotName, this.robotName = robotName; this.fullRobotModel = fullRobotModel; - ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); LogTools.info("Creating stereo point cloud publisher. Topic name: {}", topic.getName()); pointcloudPublisher = ros2Node.createPublisher(topic)::publish; diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/supportingPlanarRegionPublisher/BipedalSupportPlanarRegionPublisher.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/supportingPlanarRegionPublisher/BipedalSupportPlanarRegionPublisher.java index bd650436dc83..5cf25ffc0bdf 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/supportingPlanarRegionPublisher/BipedalSupportPlanarRegionPublisher.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/supportingPlanarRegionPublisher/BipedalSupportPlanarRegionPublisher.java @@ -12,7 +12,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.commons.thread.ThreadTools; import us.ihmc.communication.HumanoidControllerAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.ROS2Tools; @@ -59,9 +59,10 @@ private BipedalSupportPlanarRegionPublisher(DRCRobotModel robotModel, RealtimeRO realtimeROS2Node = ROS2Tools.createRealtimeROS2Node(pubSubImplementation, "supporting_planar_region_publisher"); ros2Node = realtimeROS2Node; - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), subscriber -> latestCapturabilityBasedStatusMessage.set(subscriber.takeNextData())); - ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), subscriber -> latestRobotConfigurationData.set(subscriber.takeNextData())); regionPublisher = ros2Node.createPublisher(PerceptionAPI.BIPEDAL_SUPPORT_REGIONS); ros2Node.createSubscription(getTopic(robotName), subscriber -> latestParametersMessage.set(subscriber.takeNextData())); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/trackingCameraPublisher/TrackingCameraBridge.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/trackingCameraPublisher/TrackingCameraBridge.java index 3bf9e0d134dd..d3f654251909 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/trackingCameraPublisher/TrackingCameraBridge.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/trackingCameraPublisher/TrackingCameraBridge.java @@ -14,8 +14,9 @@ import geometry_msgs.Vector3; import us.ihmc.avatar.ros.RobotROSClockCalculator; import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.Vector3D; @@ -24,6 +25,7 @@ import us.ihmc.robotModels.FullRobotModelFactory; import us.ihmc.robotics.kinematics.TimeStampedTransform3D; import us.ihmc.ros2.ROS2NodeInterface; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataBuffer; import us.ihmc.utilities.ros.RosMainNode; @@ -61,7 +63,8 @@ public TrackingCameraBridge(String robotName, FullRobotModel fullRobotModel, ROS { this.fullRobotModel = fullRobotModel; - ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName).withTypeName(RobotConfigurationData.class), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); stampedPosePacketPublisher = ros2Node.createPublisher(PerceptionAPI.T265_POSE)::publish; } @@ -70,7 +73,8 @@ public TrackingCameraBridge(String robotName, FullRobotModel fullRobotModel, Rea { this.fullRobotModel = fullRobotModel; - realtimeROS2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + realtimeROS2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); stampedPosePacketPublisher = realtimeROS2Node.createPublisher(PerceptionAPI.T265_POSE)::publish; } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerHelper.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerHelper.java index 895b90b67e8d..aa4662c0de89 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerHelper.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerHelper.java @@ -5,7 +5,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2Input; import us.ihmc.communication.ros2.ROS2ControllerPublishSubscribeAPI; import us.ihmc.communication.ros2.ROS2Helper; @@ -20,6 +20,8 @@ public class ROS2ControllerHelper extends ROS2Helper implements ROS2ControllerPu protected final ROS2ControllerPublisherMap ros2ControllerPublisherMap; private final String simpleRobotName; + private final ROS2Topic controllerOutputTopic; + public ROS2ControllerHelper(ROS2NodeInterface ros2Node, DRCRobotModel robotModel) { this(ros2Node, robotModel.getSimpleRobotName()); @@ -30,6 +32,8 @@ public ROS2ControllerHelper(ROS2NodeInterface ros2Node, String simpleRobotName) super(ros2Node); this.simpleRobotName = simpleRobotName; ros2ControllerPublisherMap = new ROS2ControllerPublisherMap(simpleRobotName, ros2PublisherMap); + + controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(simpleRobotName); } @Override @@ -59,19 +63,19 @@ public void publish(Function> topic, T message) @Override public ROS2Input subscribeToController(Class messageClass) { - return subscribe(HumanoidControllerAPI.getTopic(messageClass, simpleRobotName)); + return subscribe(ControllerAPI.getTopic(controllerOutputTopic, messageClass)); } @Override public ROS2Input subscribeToRobotConfigurationData() { - return subscribe(StateEstimatorAPI.getRobotConfigurationDataTopic(getRobotName())); + return subscribe(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class)); } @Override public void subscribeToControllerViaCallback(Class messageClass, Consumer callback) { - subscribeViaCallback(HumanoidControllerAPI.getTopic(messageClass, simpleRobotName), callback); + subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, messageClass), callback); } @Override diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerPublisherMap.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerPublisherMap.java index 51851adc0ae6..fa76e4733e4b 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerPublisherMap.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros2/ROS2ControllerPublisherMap.java @@ -1,6 +1,7 @@ package us.ihmc.avatar.ros2; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2PublisherMap; import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.ROS2NodeInterface; @@ -9,9 +10,9 @@ public class ROS2ControllerPublisherMap { - private final String robotName; private final ROS2PublisherMap publisherMap; private final HashMap topicMap = new HashMap<>(); + private final ROS2Topic controllerInputTopic; public ROS2ControllerPublisherMap(ROS2NodeInterface ros2Node, String robotName) { @@ -20,8 +21,9 @@ public ROS2ControllerPublisherMap(ROS2NodeInterface ros2Node, String robotName) public ROS2ControllerPublisherMap(String robotName, ROS2PublisherMap ros2PublisherMap) { - this.robotName = robotName; this.publisherMap = ros2PublisherMap; + + controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName); } public void publish(Object message) @@ -29,7 +31,7 @@ public void publish(Object message) ROS2Topic topic = topicMap.get(message.getClass()); if (topic == null) { - topic = HumanoidControllerAPI.getTopic(message.getClass(), robotName); + topic = ControllerAPI.getTopic(controllerInputTopic, message.getClass()); topicMap.put(message.getClass(), topic); } publisherMap.publish(topic, message); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java index 6c7f7dcb8c58..6d8336fd5640 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java @@ -25,7 +25,8 @@ import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.ComponentBasedFootstepDataMessageGeneratorFactory; import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPluginFactory; import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.JoystickBasedSteppingPluginFactory; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler.TaskOverrunBehavior; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.graphicsDescription.HeightMap; @@ -44,6 +45,7 @@ import us.ihmc.robotics.physics.RobotCollisionModel; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.scs2.SimulationConstructionSet2; import us.ihmc.scs2.definition.controller.ControllerInput; @@ -385,7 +387,8 @@ private void setupStateEstimationThread() if (realtimeROS2Node.hasBeenSet()) { pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeROS2Node.get(), robotName); - realtimeROS2Node.get().createSubscription(StateEstimatorAPI.getTopic(StampedPosePacket.class, robotName), + ROS2Topic controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName); + realtimeROS2Node.get().createSubscription(ControllerAPI.getTopic(controllerInputTopic, StampedPosePacket.class), s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData())); } } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/realsense/DelayFixedPlanarRegionsSubscription.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/realsense/DelayFixedPlanarRegionsSubscription.java index a67afbf99770..9b7df2475c8e 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/realsense/DelayFixedPlanarRegionsSubscription.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/realsense/DelayFixedPlanarRegionsSubscription.java @@ -6,8 +6,9 @@ import org.apache.commons.lang3.tuple.Pair; import org.ros.message.Time; import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.perception.filters.CollidingScanRegionFilter; import us.ihmc.avatar.ros.RobotROSClockCalculator; import us.ihmc.commons.Conversions; @@ -28,6 +29,7 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2NodeInterface; import us.ihmc.ros2.ROS2Subscription; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataBuffer; import us.ihmc.tools.thread.MissingThreadTools; import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; @@ -78,7 +80,8 @@ public DelayFixedPlanarRegionsSubscription(ROS2NodeInterface ros2Node, this.callback = callback; rosClockCalculator = robotModel.getROSClockCalculator(); - ros2Node.createSubscription2(StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + ros2Node.createSubscription2(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), rosClockCalculator::receivedRobotConfigurationData); ros2Node.createSubscription2(PerceptionAPI.MAPSENSE_REGIONS_DELAY_OFFSET, message -> delayOffset.setValue(message.getData())); @@ -226,7 +229,8 @@ public void setEnabled(boolean enabled) { if (enabled) { - robotConfigurationDataSubscriber = ros2Node.createSubscription2(StateEstimatorAPI.getRobotConfigurationDataTopic(robotModel.getSimpleRobotName()), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + robotConfigurationDataSubscriber = ros2Node.createSubscription2(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), this::acceptRobotConfigurationData); } else diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxControllerTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxControllerTest.java index 8f289857f019..f2b0aaad2b4f 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxControllerTest.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxControllerTest.java @@ -30,7 +30,6 @@ import us.ihmc.commons.ContinuousIntegrationTools; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ROS2Tools; -import us.ihmc.communication.StateEstimatorAPI; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; @@ -185,7 +184,7 @@ public void setupWithWalkingController(IKStreamingTestRunParameters ikStreamingT RobotConfigurationDataBasedUpdater robotStateUpdater = new RobotConfigurationDataBasedUpdater(); toolboxController.setRobotStateUpdater(robotStateUpdater); - ROS2Subscription rcdSubscription = toolboxROS2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + ROS2Subscription rcdSubscription = toolboxROS2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> robotStateUpdater.setRobotConfigurationData(s.takeNextData())); cleanupTasks.add(rcdSubscription::remove); diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java index d416cd7e7e83..23e71576d56e 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java @@ -19,7 +19,6 @@ import us.ihmc.commons.thread.ThreadTools; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ROS2Tools; -import us.ihmc.communication.StateEstimatorAPI; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; @@ -201,7 +200,7 @@ public void createToolboxController(DRCRobotModel robotModel) RobotConfigurationDataBasedUpdater robotStateUpdater = new RobotConfigurationDataBasedUpdater(); toolboxController.setRobotStateUpdater(robotStateUpdater); - toolboxROS2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), + toolboxROS2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> robotStateUpdater.setRobotConfigurationData(s.takeNextData())); toolboxROS2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/testTools/scs2/SCS2AvatarTestingSimulationFactory.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/testTools/scs2/SCS2AvatarTestingSimulationFactory.java index e3ad8eb871a1..c8765b36f172 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/testTools/scs2/SCS2AvatarTestingSimulationFactory.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/testTools/scs2/SCS2AvatarTestingSimulationFactory.java @@ -11,6 +11,7 @@ import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.StepGeneratorAPIDefinition; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ROS2Tools; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.controllerAPI.command.Command; import us.ihmc.communication.net.ObjectConsumer; import us.ihmc.pubsub.DomainFactory.PubSubImplementation; @@ -25,6 +26,7 @@ import us.ihmc.tools.factories.OptionalFactoryField; import us.ihmc.yoVariables.exceptions.IllegalOperationException; +import javax.naming.ldap.Control; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -36,7 +38,8 @@ public class SCS2AvatarTestingSimulationFactory extends SCS2AvatarSimulationFact private final PubSubImplementation pubSubImplementation = PubSubImplementation.INTRAPROCESS; private final ROS2Node ros2Node = ROS2Tools.createROS2Node(pubSubImplementation, "ihmc_simulation_test_helper"); - + private final ROS2Topic controllerInputTopic; + private final ROS2Topic controllerOutputTopic; @SuppressWarnings("rawtypes") private final Map, ROS2PublisherBasics> defaultControllerPublishers = new HashMap<>(); @@ -77,15 +80,20 @@ public SCS2AvatarTestingSimulationFactory(DRCRobotModel robotModel, CommonAvatar setRobotModel(robotModel); setCommonAvatarEnvrionmentInterface(environment); + controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotModel.getSimpleRobotName()); + controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + setRealtimeROS2Node(ROS2Tools.createRealtimeROS2Node(pubSubImplementation, "ihmc_simulation")); List>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); String robotName = this.robotModel.get().getSimpleRobotName(); + ROS2Topic inputTopic = HumanoidControllerAPI.getInputTopic(robotName); + for (Class> command : controllerSupportedCommands) { Class messageClass = ROS2TopicNameTools.newMessageInstance(command).getMessageClass(); - defaultControllerPublishers.put(messageClass, ros2Node.createPublisher(HumanoidControllerAPI.getTopic(messageClass, robotName))); + defaultControllerPublishers.put(messageClass, ros2Node.createPublisher(ControllerAPI.getTopic(inputTopic, messageClass))); } List>> stepGeneratorSupportedCommands = StepGeneratorAPIDefinition.getStepGeneratorSupportedCommands(); @@ -97,13 +105,14 @@ public SCS2AvatarTestingSimulationFactory(DRCRobotModel robotModel, CommonAvatar } defaultControllerPublishers.put(WholeBodyTrajectoryMessage.class, - ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyTrajectoryMessage.class, robotName))); + ros2Node.createPublisher(ControllerAPI.getTopic(inputTopic, WholeBodyTrajectoryMessage.class))); defaultControllerPublishers.put(WholeBodyStreamingMessage.class, - ros2Node.createPublisher(HumanoidControllerAPI.getTopic(WholeBodyStreamingMessage.class, robotName))); - defaultControllerPublishers.put(MessageCollection.class, ros2Node.createPublisher(HumanoidControllerAPI.getTopic(MessageCollection.class, robotName))); + ros2Node.createPublisher(ControllerAPI.getTopic(inputTopic, WholeBodyStreamingMessage.class))); + defaultControllerPublishers.put(MessageCollection.class, ros2Node.createPublisher(ControllerAPI.getTopic(inputTopic, MessageCollection.class))); + // TODO see if this can be replaced with {@link ControllerAPI#getTopic()} defaultControllerPublishers.put(ValkyrieHandFingerTrajectoryMessage.class, - createPublisher(ValkyrieHandFingerTrajectoryMessage.class, HumanoidControllerAPI.getInputTopic(robotName))); + createPublisher(ValkyrieHandFingerTrajectoryMessage.class, inputTopic)); } public SCS2AvatarTestingSimulation createAvatarTestingSimulation() @@ -167,7 +176,7 @@ public ROS2PublisherBasics createPublisher(Class messageType, ROS2Topi public void createSubscriberFromController(Class messageType, ObjectConsumer consumer) { - createSubscriber(messageType, HumanoidControllerAPI.getTopic(messageType, robotModel.get().getSimpleRobotName()), consumer); + createSubscriber(messageType, ControllerAPI.getTopic(controllerOutputTopic, messageType), consumer); } public void createSubscriber(Class messageType, ROS2Topic generator, ObjectConsumer consumer) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java index 5a844bad24f7..feb4edde0a55 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java @@ -24,11 +24,15 @@ import java.util.Map; import controller_msgs.msg.dds.*; +import ihmc_common_msgs.msg.dds.MessageCollectionNotification; +import ihmc_common_msgs.msg.dds.TextToSpeechPacket; import perception_msgs.msg.dds.PlanarRegionsListMessage; +import toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus; import us.ihmc.commonWalkingControlModules.controllerAPI.input.ControllerNetworkSubscriber.MessageValidator; import us.ihmc.commonWalkingControlModules.controllerAPI.input.MessageCollector.MessageIDExtractor; import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.controllerAPI.command.Command; +import us.ihmc.communication.packets.Packet; import us.ihmc.euclid.interfaces.Settable; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.*; import us.ihmc.humanoidRobotics.communication.directionalControlToolboxAPI.DirectionalControlInputCommand; @@ -98,7 +102,37 @@ public class ControllerAPIDefinition controllerSupportedCommands = Collections.unmodifiableList(commands); - controllerSupportedStatusMessages = ControllerAPI.outputMessageClasses.stream().toList(); + List>> statusMessages = new ArrayList<>(); + + + // Statuses supported by bipedal walking controller {@link WalkingControllerState} + statusMessages.add(CapturabilityBasedStatus.class); + statusMessages.add(FootstepStatusMessage.class); + statusMessages.add(PlanOffsetStatus.class); + statusMessages.add(WalkingStatusMessage.class); + statusMessages.add(WalkingControllerFailureStatusMessage.class); + statusMessages.add(ManipulationAbortedStatus.class); + statusMessages.add(HighLevelStateChangeStatusMessage.class); + statusMessages.add(TextToSpeechPacket.class); + statusMessages.add(ControllerCrashNotificationPacket.class); + statusMessages.add(JointspaceTrajectoryStatusMessage.class); + statusMessages.add(TaskspaceTrajectoryStatusMessage.class); + statusMessages.add(JointDesiredOutputMessage.class); + statusMessages.add(RobotDesiredConfigurationData.class); + statusMessages.add(FootstepQueueStatusMessage.class); + statusMessages.add(QueuedFootstepStatusMessage.class); + statusMessages.add(WrenchTrajectoryStatusMessage.class); + statusMessages.add(InvalidPacketNotificationPacket.class); + statusMessages.add(MessageCollectionNotification.class); + + // Statuses supported by the kinematics toolbox + statusMessages.add(KinematicsToolboxOutputStatus.class); + + // Statuses supported by multi-contact controller, not in this repo + statusMessages.add(MultiContactBalanceStatus.class); + statusMessages.add(MultiContactTrajectoryStatus.class); + + controllerSupportedStatusMessages = Collections.unmodifiableList(statusMessages); } public static List>> getControllerSupportedCommands() diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/HumanoidControllerAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/HumanoidControllerAPI.java index 1b179a67c9e6..bb3b5ae745b0 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/HumanoidControllerAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/HumanoidControllerAPI.java @@ -1,31 +1,49 @@ package us.ihmc.communication; +import controller_msgs.msg.dds.*; +import ihmc_common_msgs.msg.dds.MessageCollection; +import ihmc_common_msgs.msg.dds.MessageCollectionNotification; import ihmc_common_msgs.msg.dds.TextToSpeechPacket; +import toolbox_msgs.msg.dds.*; import us.ihmc.communication.controllerAPI.ControllerAPI; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.ros2.ROS2QosProfile; import us.ihmc.ros2.ROS2Topic; +import java.util.*; + public final class HumanoidControllerAPI { public static final String HUMANOID_CONTROLLER_NODE_NAME = "ihmc_controller"; public static final String HUMANOID_KINEMATICS_CONTROLLER_NODE_NAME = "kinematics_ihmc_controller"; public static final String HUMANOID_CONTROL_MODULE_NAME = "humanoid_control"; + /** + * Please use the {@link #getOutputTopic(String)} and {@link #getInputTopic(String)} messages instead. + */ + @Deprecated public static final ROS2Topic HUMANOID_CONTROLLER = ROS2Tools.IHMC_ROOT.withModule(HUMANOID_CONTROL_MODULE_NAME); public static final ROS2Topic TEXT_STATUS = ROS2Tools.IHMC_ROOT.withTypeName(TextToSpeechPacket.class); + + + /** + * Gets the base topic name for status messages that are coming from the robot. + * @param robotName + * @return + */ public static ROS2Topic getOutputTopic(String robotName) { return HUMANOID_CONTROLLER.withRobot(robotName).withOutput(); } + /** + * Gets the base topic name for commands that are going to the robot. + * @param robotName + * @return + */ public static ROS2Topic getInputTopic(String robotName) { return HUMANOID_CONTROLLER.withRobot(robotName).withInput(); } - - /** Applies only for the humanoid controller. */ - public static ROS2Topic getTopic(Class messageClass, String robotName) - { - return ControllerAPI.getTopic(ControllerAPI.getBaseTopic(HUMANOID_CONTROL_MODULE_NAME, robotName), messageClass); - } } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ROS2Tools.java b/ihmc-communication/src/main/java/us/ihmc/communication/ROS2Tools.java index eebee733a34b..dccd20eb11ea 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/ROS2Tools.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ROS2Tools.java @@ -31,7 +31,6 @@ *
  • {@link MissionControlAPI}
    • *
  • {@link PerceptionAPI}
    • *
  • {@link SakeHandAPI}
    • - *
  • {@link StateEstimatorAPI}
    • *
  • {@link ToolboxAPIs}
    • * * diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/StateEstimatorAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/StateEstimatorAPI.java deleted file mode 100644 index 2b15c2eefa08..000000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/StateEstimatorAPI.java +++ /dev/null @@ -1,51 +0,0 @@ -package us.ihmc.communication; - -import controller_msgs.msg.dds.HandJointAnglePacket; -import controller_msgs.msg.dds.LocalizationPacket; -import controller_msgs.msg.dds.PelvisPoseErrorPacket; -import controller_msgs.msg.dds.RobotConfigurationData; -import ihmc_common_msgs.msg.dds.StampedPosePacket; -import us.ihmc.ros2.ROS2QosProfile; -import us.ihmc.ros2.ROS2Topic; - -/** - * TODO: Does this category make sense? Should these go in {@link HumanoidControllerAPI}? - */ -public final class StateEstimatorAPI -{ - public static ROS2Topic getHandJointAnglesTopic(String robotName) - { - return getTopic(HandJointAnglePacket.class, robotName); - } - - public static ROS2Topic getRobotConfigurationDataTopic(String robotName) - { - return getRobotConfigurationDataTopic(HumanoidControllerAPI.getOutputTopic(robotName)); - } - - public static ROS2Topic getRobotConfigurationDataTopic(ROS2Topic outputTopic) - { - return outputTopic.withTypeName(RobotConfigurationData.class).withQoS(ROS2QosProfile.BEST_EFFORT()); - } - - public static ROS2Topic getTopic(Class messageClass, String robotName) - { - // Input types - if (messageClass.equals(StampedPosePacket.class) - || messageClass.equals(PelvisPoseErrorPacket.class) - || messageClass.equals(LocalizationPacket.class)) - { - return HumanoidControllerAPI.getInputTopic(robotName).withTypeName(messageClass).withQoS(ROS2QosProfile.BEST_EFFORT()); - } - // Output types - else if (messageClass.equals(HandJointAnglePacket.class) - || messageClass.equals(RobotConfigurationData.class)) - { - return HumanoidControllerAPI.getOutputTopic(robotName).withTypeName(messageClass).withQoS(ROS2QosProfile.BEST_EFFORT()); - } - else - { - throw new RuntimeException("Message class is not part of the state estimator API: %s".formatted(messageClass.getName())); - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java index 5bed4143949d..5caf97b0449c 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java @@ -3,6 +3,7 @@ import controller_msgs.msg.dds.*; import ihmc_common_msgs.msg.dds.MessageCollection; import ihmc_common_msgs.msg.dds.MessageCollectionNotification; +import ihmc_common_msgs.msg.dds.StampedPosePacket; import ihmc_common_msgs.msg.dds.TextToSpeechPacket; import toolbox_msgs.msg.dds.HumanoidKinematicsToolboxConfigurationMessage; import toolbox_msgs.msg.dds.KinematicsStreamingToolboxConfigurationMessage; @@ -16,7 +17,6 @@ import toolbox_msgs.msg.dds.KinematicsToolboxPrivilegedConfigurationMessage; import toolbox_msgs.msg.dds.KinematicsToolboxRigidBodyMessage; import toolbox_msgs.msg.dds.KinematicsToolboxSupportRegionMessage; -import us.ihmc.communication.ROS2Tools; import us.ihmc.euclid.interfaces.Settable; import us.ihmc.ros2.ROS2QosProfile; import us.ihmc.ros2.ROS2Topic; @@ -34,143 +34,42 @@ */ public final class ControllerAPI { - public static final Set> inputMessageClasses = new HashSet<>(); - public static final Set>> outputMessageClasses = new HashSet<>(); - - public static final Map, ROS2QosProfile> inputMessageClassSpecificQoS = new HashMap<>(); - public static final Map, ROS2QosProfile> outputMessageClassSpecificQoS = new HashMap<>(); + /** + * TODO {@link #getTopic(ROS2Topic, Class)} should be reconfigured to consume a custom QoS provider, or at least this should be configurable for different + * platforms. As it stands, this implementation applies to every robot that uses this API, which is likely a mistake, particularly since these are messages + * that are only applicable to the humanoid controllers. + */ + private static final Map, ROS2QosProfile> messageClassSpecificQoS = new HashMap<>(); static { - // Commands supported by bipedal walking controller WalkingControllerState - inputMessageClasses.add(ArmTrajectoryMessage.class); - inputMessageClasses.add(HandTrajectoryMessage.class); - inputMessageClasses.add(LegTrajectoryMessage.class); - inputMessageClasses.add(FootTrajectoryMessage.class); - inputMessageClasses.add(HeadTrajectoryMessage.class); - inputMessageClasses.add(NeckTrajectoryMessage.class); - inputMessageClasses.add(NeckDesiredAccelerationsMessage.class); - inputMessageClasses.add(ChestTrajectoryMessage.class); - inputMessageClasses.add(SpineTrajectoryMessage.class); - inputMessageClasses.add(PelvisTrajectoryMessage.class); - inputMessageClasses.add(PelvisOrientationTrajectoryMessage.class); - inputMessageClasses.add(PelvisHeightTrajectoryMessage.class); - inputMessageClasses.add(StopAllTrajectoryMessage.class); - inputMessageClasses.add(FootstepDataListMessage.class); - inputMessageClasses.add(GoHomeMessage.class); - inputMessageClasses.add(FootLoadBearingMessage.class); - inputMessageClasses.add(ArmDesiredAccelerationsMessage.class); - inputMessageClasses.add(AutomaticManipulationAbortMessage.class); - inputMessageClasses.add(HighLevelStateMessage.class); - inputMessageClasses.add(AbortWalkingMessage.class); - inputMessageClasses.add(PrepareForLocomotionMessage.class); - inputMessageClasses.add(PauseWalkingMessage.class); - inputMessageClasses.add(SpineDesiredAccelerationsMessage.class); - inputMessageClasses.add(HandLoadBearingMessage.class); - inputMessageClasses.add(HandHybridJointspaceTaskspaceTrajectoryMessage.class); - inputMessageClasses.add(HeadHybridJointspaceTaskspaceTrajectoryMessage.class); - inputMessageClasses.add(ChestHybridJointspaceTaskspaceTrajectoryMessage.class); - inputMessageClasses.add(ClearDelayQueueMessage.class); - inputMessageClasses.add(MomentumTrajectoryMessage.class); - inputMessageClasses.add(CenterOfMassTrajectoryMessage.class); - inputMessageClasses.add(HandWrenchTrajectoryMessage.class); - - // Commands supported by the fast-walking controller, not in this repo - inputMessageClasses.add(DirectionalControlInputMessage.class); - inputMessageClasses.add(FastWalkingGaitParametersMessage.class); - - // Commands supported by multi-contact controller, not in this repo - inputMessageClasses.add(MultiContactTrajectoryMessage.class); - inputMessageClasses.add(MultiContactTrajectorySequenceMessage.class); - inputMessageClasses.add(MultiContactBalanceStatus.class); - inputMessageClasses.add(MultiContactTimedContactSequenceMessage.class); - - // Commands supported by the Crocoddyl control state - inputMessageClasses.add(CrocoddylSolverTrajectoryMessage.class); - - // Command supported by the joint-space controller JointspacePositionControllerState - inputMessageClasses.add(WholeBodyJointspaceTrajectoryMessage.class); - - // Commands supported by the kinematics toolbox - inputMessageClasses.add(KinematicsToolboxCenterOfMassMessage.class); - inputMessageClasses.add(KinematicsToolboxRigidBodyMessage.class); - inputMessageClasses.add(KinematicsToolboxOneDoFJointMessage.class); - inputMessageClasses.add(KinematicsToolboxConfigurationMessage.class); - inputMessageClasses.add(KinematicsToolboxSupportRegionMessage.class); - inputMessageClasses.add(KinematicsToolboxPrivilegedConfigurationMessage.class); - inputMessageClasses.add(KinematicsToolboxInitialConfigurationMessage.class); - inputMessageClasses.add(KinematicsToolboxInputCollectionMessage.class); - inputMessageClasses.add(HumanoidKinematicsToolboxConfigurationMessage.class); - - // Commands supported by the kinematics streaming toolbox - inputMessageClasses.add(KinematicsStreamingToolboxInputMessage.class); - inputMessageClasses.add(KinematicsStreamingToolboxConfigurationMessage.class); - - // Input messages that don't have a corresponding command - inputMessageClasses.add(MessageCollection.class); - inputMessageClasses.add(WholeBodyTrajectoryMessage.class); - inputMessageClasses.add(WholeBodyStreamingMessage.class); - - // Statuses supported by bipedal walking controller {@link WalkingControllerState} - outputMessageClasses.add(CapturabilityBasedStatus.class); - outputMessageClasses.add(FootstepStatusMessage.class); - outputMessageClasses.add(PlanOffsetStatus.class); - outputMessageClasses.add(WalkingStatusMessage.class); - outputMessageClasses.add(WalkingControllerFailureStatusMessage.class); - outputMessageClasses.add(ManipulationAbortedStatus.class); - outputMessageClasses.add(HighLevelStateChangeStatusMessage.class); - outputMessageClasses.add(TextToSpeechPacket.class); - outputMessageClasses.add(ControllerCrashNotificationPacket.class); - outputMessageClasses.add(JointspaceTrajectoryStatusMessage.class); - outputMessageClasses.add(TaskspaceTrajectoryStatusMessage.class); - outputMessageClasses.add(JointDesiredOutputMessage.class); - outputMessageClasses.add(RobotDesiredConfigurationData.class); - outputMessageClasses.add(FootstepQueueStatusMessage.class); - outputMessageClasses.add(QueuedFootstepStatusMessage.class); - outputMessageClasses.add(WrenchTrajectoryStatusMessage.class); - outputMessageClasses.add(InvalidPacketNotificationPacket.class); - outputMessageClasses.add(MessageCollectionNotification.class); - - // Statuses supported by the kinematics toolbox - outputMessageClasses.add(KinematicsToolboxOutputStatus.class); - - // Statuses supported by multi-contact controller, not in this repo - outputMessageClasses.add(MultiContactBalanceStatus.class); - outputMessageClasses.add(MultiContactTrajectoryStatus.class); - // Setting the input messages with specific QoS - inputMessageClassSpecificQoS.put(WholeBodyStreamingMessage.class, ROS2QosProfile.BEST_EFFORT()); - inputMessageClassSpecificQoS.put(KinematicsStreamingToolboxInputMessage.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(WholeBodyStreamingMessage.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(KinematicsStreamingToolboxInputMessage.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(StampedPosePacket.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(PelvisPoseErrorPacket.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(LocalizationPacket.class, ROS2QosProfile.BEST_EFFORT()); // Setting the output messages with specific QoS - outputMessageClassSpecificQoS.put(CapturabilityBasedStatus.class, ROS2QosProfile.BEST_EFFORT()); - outputMessageClassSpecificQoS.put(JointDesiredOutputMessage.class, ROS2QosProfile.BEST_EFFORT()); - outputMessageClassSpecificQoS.put(RobotDesiredConfigurationData.class, ROS2QosProfile.BEST_EFFORT()); - outputMessageClassSpecificQoS.put(FootstepQueueStatusMessage.class, ROS2QosProfile.BEST_EFFORT()); - outputMessageClassSpecificQoS.put(MultiContactBalanceStatus.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(CapturabilityBasedStatus.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(JointDesiredOutputMessage.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(RobotDesiredConfigurationData.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(FootstepQueueStatusMessage.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(MultiContactBalanceStatus.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(HandJointAnglePacket.class, ROS2QosProfile.BEST_EFFORT()); + messageClassSpecificQoS.put(RobotConfigurationData.class, ROS2QosProfile.BEST_EFFORT()); } - public static ROS2Topic getBaseTopic(String controlModuleName, String robotName) - { - return ROS2Tools.IHMC_ROOT.withModule(controlModuleName).withRobot(robotName); - } public static ROS2Topic getTopic(ROS2Topic baseTopic, Class messageClass) { - if (inputMessageClasses.contains(messageClass)) - return baseTopic.withInput().withTypeName(messageClass).withQoS(getQoS(messageClass)); - else if (outputMessageClasses.contains(messageClass)) - return baseTopic.withOutput().withTypeName(messageClass).withQoS(getQoS(messageClass)); - else - return baseTopic.withTypeName(messageClass).withQoS(getQoS(messageClass)); + return baseTopic.withTypeName(messageClass).withQoS(getQoS(messageClass)); } public static ROS2QosProfile getQoS(Class messageClass) { - if (inputMessageClasses.contains(messageClass)) - return Objects.requireNonNullElse(inputMessageClassSpecificQoS.get(messageClass), ROS2QosProfile.RELIABLE()); - else if (outputMessageClasses.contains(messageClass)) - return Objects.requireNonNullElse(outputMessageClassSpecificQoS.get(messageClass), ROS2QosProfile.RELIABLE()); + if (messageClassSpecificQoS.containsKey(messageClass)) + return Objects.requireNonNullElse(messageClassSpecificQoS.get(messageClass), ROS2QosProfile.RELIABLE()); else return ROS2QosProfile.DEFAULT(); } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/util/RobotTimeBasedExecutorService.java b/ihmc-communication/src/main/java/us/ihmc/communication/util/RobotTimeBasedExecutorService.java index b526fd3113d4..eac8df7baf8a 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/util/RobotTimeBasedExecutorService.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/util/RobotTimeBasedExecutorService.java @@ -11,6 +11,7 @@ import controller_msgs.msg.dds.RobotConfigurationData; import us.ihmc.commons.Conversions; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.*; import us.ihmc.log.LogTools; import us.ihmc.pubsub.common.SampleInfo; @@ -44,7 +45,7 @@ public class RobotTimeBasedExecutorService public static void schedulePackageBased(RealtimeROS2Node ros2Node, String robotName, long period, TimeUnit timeUnit, Runnable runnable) { NewMessageListener robotConfigurationDataListener = createListener(period, timeUnit, runnable); - ros2Node.createSubscription(createTopicName(robotName).withTypeName(RobotConfigurationData.class), robotConfigurationDataListener); + ros2Node.createSubscription(ControllerAPI.getTopic(HumanoidControllerAPI.getOutputTopic(robotName), RobotConfigurationData.class), robotConfigurationDataListener); } /** @@ -61,12 +62,7 @@ public static void schedulePackageBased(RealtimeROS2Node ros2Node, String robotN public static void schedulePackageBased(ROS2Node ros2Node, String robotName, long period, TimeUnit timeUnit, Runnable runnable) { NewMessageListener robotConfigurationDataListener = createListener(period, timeUnit, runnable); - ros2Node.createSubscription(createTopicName(robotName).withTypeName(RobotConfigurationData.class), robotConfigurationDataListener); - } - - private static ROS2Topic createTopicName(String robotName) - { - return HumanoidControllerAPI.HUMANOID_CONTROLLER.withRobot(robotName).withOutput(); + ros2Node.createSubscription(ControllerAPI.getTopic(HumanoidControllerAPI.getOutputTopic(robotName), RobotConfigurationData.class), robotConfigurationDataListener); } private static NewMessageListener createListener(long period, TimeUnit timeUnit, Runnable runnable) @@ -161,8 +157,7 @@ public static void schedulePackageBasedWithDelayCompensation(RealtimeROS2Node ro AtomicDouble estimatedRealtimeRate = new AtomicDouble(1.0); // Create a thread that estimates the current realtime rate. - ROS2Topic topicName = createTopicName(robotName); - ros2Node.createSubscription(((ROS2Topic) topicName).withTypeName(RobotConfigurationData.class), new NewMessageListener() + ros2Node.createSubscription(ControllerAPI.getTopic(HumanoidControllerAPI.getOutputTopic(robotName), RobotConfigurationData.class), new NewMessageListener() { private final SampleInfo sampleInfo = new SampleInfo(); private final RobotConfigurationData robotConfigurationData = new RobotConfigurationData(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 4bcbe3bda6fc..2a49fcf53093 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -20,6 +20,7 @@ import us.ihmc.commonWalkingControlModules.configurations.SwingTrajectoryParameters; import us.ihmc.commonWalkingControlModules.trajectories.PositionOptimizedTrajectoryGenerator; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; @@ -51,6 +52,7 @@ import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2PublisherBasics; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.tools.property.StoredPropertySetBasics; @@ -132,7 +134,8 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper monteCarloPlannerParameters, robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, WalkingControllerFailureStatusMessage.class), message -> terrainPlanningDebugger.reset()); remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java index 37f84fdad3ae..9d05d972025e 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java @@ -5,13 +5,14 @@ import com.badlogic.gdx.utils.Array; import com.badlogic.gdx.utils.Pool; import controller_msgs.msg.dds.FootstepStatusMessage; +import controller_msgs.msg.dds.RobotConfigurationData; import imgui.ImGui; import imgui.type.ImBoolean; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.tools.MinimalFootstep; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -33,6 +34,7 @@ import us.ihmc.robotics.EuclidCoreMissingTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.ros2.ROS2Topic; import java.util.ArrayList; import java.util.List; @@ -79,7 +81,8 @@ public RDXROS2RobotVisualizer(RDXBaseUI baseUI, ROS2SyncedRobotModel syncedRobot, Supplier cameraForTrackingSupplier) { - super(syncedRobot.getRobotModel().getSimpleRobotName() + " Robot Visualizer", StateEstimatorAPI.getRobotConfigurationDataTopic(syncedRobot.getRobotModel().getSimpleRobotName())); + super(syncedRobot.getRobotModel().getSimpleRobotName() + " Robot Visualizer", + ControllerAPI.getTopic(HumanoidControllerAPI.getOutputTopic(syncedRobot.getRobotModel().getSimpleRobotName()), RobotConfigurationData.class)); this.baseUI = baseUI; this.ros2 = ros2; this.syncedRobot = syncedRobot; @@ -151,7 +154,8 @@ public void create() PerceptionAPI.EXPERIMENTAL_CAMERA_TO_PARENT_TUNING, syncedRobot.getRobotModel().getSensorInformation().getExperimentalCameraTransform()); - ros2.subscribeViaVolatileCallback(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, syncedRobot.getRobotModel().getSimpleRobotName()), footstepStatusMessage -> + ROS2Topic controllereOutputTopic = HumanoidControllerAPI.getOutputTopic(syncedRobot.getRobotModel().getSimpleRobotName()); + ros2.subscribeViaVolatileCallback(ControllerAPI.getTopic(controllereOutputTopic, FootstepStatusMessage.class), footstepStatusMessage -> { if (footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_COMPLETED) completedFootstepThreadBarrier.add(new MinimalFootstep(footstepStatusMessage)); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java index 653c671b67e5..44b2da1a062f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java @@ -7,6 +7,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask.PlanningMode; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2PublisherMap; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; @@ -50,15 +51,19 @@ public ActivePlanarMappingRemoteTask(String simpleRobotName, this.terrainPlanningDebugger = new TerrainPlanningDebugger(ros2Node, null, PlanningMode.FAST_HIKING); this.continuousPlanningParameters = continuousPlanningParameters; this.swingFootPlannerParameters = robotModel.getSwingPlannerParameters(); - this.controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, robotModel.getSimpleRobotName()); continuousPlanner = new ContinuousPlannerForPlanarRegions(referenceFrames); publisherMap = new ROS2PublisherMap(ros2Node); - publisherMap.getOrCreatePublisher(controllerFootstepDataTopic); ros2Helper.subscribeViaCallback(terrainRegionsTopic, this::onPlanarRegionsReceived); //ros2Helper.subscribeViaCallback(PerceptionAPI.OUSTER_DEPTH_IMAGE, this::onOusterDepthReceived); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, robotModel.getSimpleRobotName()), this::walkingStatusReceived); + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + ROS2Topic controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotModel.getSimpleRobotName()); + + this.controllerFootstepDataTopic = ControllerAPI.getTopic(controllerInputTopic, FootstepDataListMessage.class); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, WalkingStatusMessage.class), this::walkingStatusReceived); + + publisherMap.getOrCreatePublisher(controllerFootstepDataTopic); executorService.scheduleAtFixedRate(this::updateActiveMappingPlan, 0, PLANNING_PERIOD_MS, TimeUnit.MILLISECONDS); executorService.scheduleAtFixedRate(this::generalUpdate, 0, UPDATE_PERIOD_MS, TimeUnit.MILLISECONDS); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java index d4bfd2dbe1d5..db5ef2c7b972 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java @@ -3,10 +3,12 @@ import controller_msgs.msg.dds.PauseWalkingMessage; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.behaviors.activeMapping.TerrainPlanningDebugger; import us.ihmc.robotics.stateMachine.core.State; import us.ihmc.ros2.ROS2PublisherBasics; +import us.ihmc.ros2.ROS2Topic; public class DoNothingState implements State { @@ -25,7 +27,8 @@ public DoNothingState(ROS2Helper ros2Helper, String simpleRobotName, ContinuousP this.continuousPlanner = continuousPlanner; this.debugger = debugger; - pauseWalkingPublisher = ros2Helper.getROS2NodeInterface().createPublisher(HumanoidControllerAPI.getTopic(PauseWalkingMessage.class, simpleRobotName)); + ROS2Topic controllerInputTopic = HumanoidControllerAPI.getInputTopic(simpleRobotName); + pauseWalkingPublisher = ros2Helper.getROS2NodeInterface().createPublisher(ControllerAPI.getTopic(controllerInputTopic, PauseWalkingMessage.class)); } @Override diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java index 1ce27d46566b..6b78ca115645 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java @@ -7,6 +7,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousPlannerStatistics; import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.log.LogTools; import us.ihmc.robotics.stateMachine.core.State; @@ -40,7 +41,8 @@ public WaitingToLandState(ROS2Helper ros2Helper, this.controllerQueueMonitor = controllerQueueMonitor; this.statistics = statistics; - controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, simpleRobotName); + ROS2Topic controllerInputTopic = HumanoidControllerAPI.getInputTopic(simpleRobotName); + controllerFootstepDataTopic = ControllerAPI.getTopic(controllerInputTopic, FootstepDataListMessage.class); ros2Helper.createPublisher(controllerFootstepDataTopic); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java index deafd94b5e5e..3b1b794070a6 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java @@ -4,10 +4,12 @@ import controller_msgs.msg.dds.FootstepStatusMessage; import controller_msgs.msg.dds.QueuedFootstepStatusMessage; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Topic; import java.util.List; import java.util.concurrent.atomic.AtomicReference; @@ -28,8 +30,9 @@ public ControllerFootstepQueueMonitor(ROS2Helper ros2Helper, { this.referenceFrames = referenceFrames; this.statistics = statistics; - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), this::footstepQueueStatusReceived); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), this::footstepStatusReceived); + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(simpleRobotName); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, FootstepQueueStatusMessage.class), this::footstepQueueStatusReceived); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, FootstepStatusMessage.class), this::footstepStatusReceived); } private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueueStatusMessage) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/lookAndStep/LookAndStepBehavior.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/lookAndStep/LookAndStepBehavior.java index 07c8a69a5bbe..ff0733134ead 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/lookAndStep/LookAndStepBehavior.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/lookAndStep/LookAndStepBehavior.java @@ -4,7 +4,6 @@ import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeStatus; import us.ihmc.behaviors.behaviorTree.ResettingNode; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.StateEstimatorAPI; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.property.ROS2StoredPropertySet; import us.ihmc.euclid.geometry.Pose3D; @@ -141,8 +140,7 @@ public LookAndStepBehavior(BehaviorHelper helper) footstepPlanning.acceptHeightMap(heightMapMessage); }); helper.subscribeViaCallback(ROS2_REGIONS_FOR_FOOTSTEP_PLANNING, footstepPlanning::acceptPlanarRegions); - helper.subscribeViaCallback(StateEstimatorAPI.getRobotConfigurationDataTopic(helper.getRobotModel().getSimpleRobotName()), - footstepPlanning::acceptRobotConfigurationData); + helper.subscribeToControllerViaCallback(RobotConfigurationData.class, footstepPlanning::acceptRobotConfigurationData); helper.subscribeToControllerViaCallback(CapturabilityBasedStatus.class, footstepPlanning::acceptCapturabilityBasedStatus); helper.subscribeToControllerViaCallback(FootstepStatusMessage.class, status -> { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/CommunicationHelper.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/CommunicationHelper.java index 00ecc36f9f96..446939a9d9ec 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/CommunicationHelper.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/CommunicationHelper.java @@ -13,7 +13,7 @@ import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.commons.thread.TypedNotification; import us.ihmc.communication.HumanoidControllerAPI; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2ControllerPublishSubscribeAPI; import us.ihmc.avatar.sensors.realsense.DelayFixedPlanarRegionsSubscription; import us.ihmc.avatar.sensors.realsense.MapsenseTools; @@ -66,10 +66,14 @@ public class CommunicationHelper implements ROS2ControllerPublishSubscribeAPI private FootstepPlanningModule footstepPlanner; private RobotLowLevelMessenger lowLevelMessenger; + private final ROS2Topic controllerOutputTopic; + public CommunicationHelper(DRCRobotModel robotModel, ROS2NodeInterface ros2Node) { this.robotModel = robotModel; this.ros2Helper = new ROS2ControllerHelper(ros2Node, robotModel); + + this.controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); } public ROS2ControllerHelper getControllerHelper() @@ -197,19 +201,19 @@ public void createPublisher(ROS2Topic topic) @Override public ROS2Input subscribeToController(Class messageClass) { - return subscribe(HumanoidControllerAPI.getTopic(messageClass, robotModel.getSimpleRobotName())); + return subscribe(ControllerAPI.getTopic(controllerOutputTopic, messageClass)); } // TODO: Move to remote robot interface? public void subscribeToControllerViaCallback(Class messageClass, Consumer callback) { - subscribeViaCallback(HumanoidControllerAPI.getTopic(messageClass, robotModel.getSimpleRobotName()), callback); + subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, messageClass), callback); } @Override public ROS2Input subscribeToRobotConfigurationData() { - return subscribe(StateEstimatorAPI.getRobotConfigurationDataTopic(getRobotName())); + return subscribe(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class)); } public void subscribeToPlanarRegionsViaCallback(ROS2Topic topic, Consumer callback) @@ -245,7 +249,7 @@ public void subscribeViaVolatileCallback(Function> topi public void subscribeToRobotConfigurationDataViaCallback(Consumer callback) { - subscribeViaCallback(StateEstimatorAPI.getRobotConfigurationDataTopic(getRobotModel().getSimpleRobotName()), callback); + subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), callback); } @Override diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/RemoteHumanoidRobotInterface.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/RemoteHumanoidRobotInterface.java index ee9996ef2902..68f35a30d895 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/RemoteHumanoidRobotInterface.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/RemoteHumanoidRobotInterface.java @@ -12,6 +12,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.communication.*; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.packets.PacketDestination; import us.ihmc.euclid.geometry.Pose3D; @@ -43,6 +44,8 @@ import us.ihmc.commons.thread.TypedNotification; import us.ihmc.robotics.partNames.HumanoidJointNameMap; +import javax.naming.ldap.Control; + // TODO: Clean this up by using DRCUserInterfaceNetworkingManager (After cleaning that up first...) public class RemoteHumanoidRobotInterface { @@ -61,7 +64,8 @@ public class RemoteHumanoidRobotInterface private final ROS2SyncedRobotModel syncedRobot; - private final ROS2Topic topicName; + private final ROS2Topic inputTopic; + private final ROS2Topic outputTopic; public RemoteHumanoidRobotInterface(ROS2NodeInterface ros2Node, DRCRobotModel robotModel) { @@ -69,19 +73,20 @@ public RemoteHumanoidRobotInterface(ROS2NodeInterface ros2Node, DRCRobotModel ro this.robotModel = robotModel; robotName = robotModel.getSimpleRobotName(); jointMap = robotModel.getJointMap(); - topicName = HumanoidControllerAPI.HUMANOID_CONTROLLER.withRobot(robotName); + inputTopic = HumanoidControllerAPI.getInputTopic(robotName); + outputTopic = HumanoidControllerAPI.getOutputTopic(robotName); controllerPublisherMap = new ROS2ControllerPublisherMap(ros2Node, robotName); publisherMap = new ROS2PublisherMap(ros2Node); - ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, robotName), this::acceptWalkingStatus); - ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, robotName), footstepStatusMessage::set); + ros2Node.createSubscription2(ControllerAPI.getTopic(outputTopic, WalkingStatusMessage.class), this::acceptWalkingStatus); + ros2Node.createSubscription2(ControllerAPI.getTopic(outputTopic, FootstepStatusMessage.class), footstepStatusMessage::set); HighLevelStateChangeStatusMessage initialState = new HighLevelStateChangeStatusMessage(); initialState.setInitialHighLevelControllerName(HighLevelControllerName.DO_NOTHING_BEHAVIOR.toByte()); initialState.setEndHighLevelControllerName(HighLevelControllerName.WALKING.toByte()); - controllerStateInput = new ROS2Input<>(ros2Node, HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), initialState, this::acceptStatusChange); - capturabilityBasedStatusInput = new ROS2Input<>(ros2Node, HumanoidControllerAPI.getTopic(CapturabilityBasedStatus.class, robotName)); + controllerStateInput = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(outputTopic, HighLevelStateChangeStatusMessage.class), initialState, this::acceptStatusChange); + capturabilityBasedStatusInput = new ROS2Input<>(ros2Node, ControllerAPI.getTopic(outputTopic, CapturabilityBasedStatus.class)); syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node); } @@ -115,7 +120,7 @@ public ROS2SyncedRobotModel newSyncedRobot() public ROS2Callback createFootstepStatusCallback(Consumer consumer) { - return new ROS2Callback<>(ros2Node, FootstepStatusMessage.class, topicName.withOutput(), consumer); + return new ROS2Callback<>(ros2Node, FootstepStatusMessage.class, outputTopic, consumer); } public FootstepStatusMessage getLatestFootstepStatusMessage() @@ -313,7 +318,7 @@ public void publishPose(Pose3D pose, double confidenceFactor, long timestamp) stampedPosePacket.setConfidenceFactor(confidenceFactor); LogTools.debug("Publishing Pose " + pose + " with timestamp " + timestamp); - publisherMap.publish(StateEstimatorAPI.getTopic(StampedPosePacket.class, robotName), stampedPosePacket); + publisherMap.publish(ControllerAPI.getTopic(inputTopic, StampedPosePacket.class), stampedPosePacket); } public void pauseWalking() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/ControllerStatusTracker.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/ControllerStatusTracker.java index 173081b34fc2..ca68a0d82671 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/ControllerStatusTracker.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/ControllerStatusTracker.java @@ -3,9 +3,11 @@ import controller_msgs.msg.dds.*; import controller_msgs.msg.dds.RobotConfigurationData; import us.ihmc.commons.thread.Notification; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2Callback; import us.ihmc.log.LogToolsWriteOnly; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.model.RobotMotionStatus; import us.ihmc.tools.Timer; import us.ihmc.euclid.tuple3D.Vector3D; @@ -17,8 +19,6 @@ import java.util.ArrayList; import java.util.List; -import static us.ihmc.communication.HumanoidControllerAPI.getTopic; - /** * A class to keep track of the controller by listening to its ROS 2 status API. *

    @@ -51,13 +51,14 @@ public ControllerStatusTracker(LogToolsWriteOnly statusLogger, ROS2NodeInterface finishedWalkingNotification.set(); - new ROS2Callback<>(ros2Node, StateEstimatorAPI.getRobotConfigurationDataTopic(robotName), this::acceptRobotConfigurationData); - new ROS2Callback<>(ros2Node, getTopic(HighLevelStateChangeStatusMessage.class, robotName), this::acceptHighLevelStateChangeStatusMessage); - new ROS2Callback<>(ros2Node, getTopic(WalkingControllerFailureStatusMessage.class, robotName), this::acceptWalkingControllerFailureStatusMessage); - new ROS2Callback<>(ros2Node, getTopic(PlanOffsetStatus.class, robotName), this::acceptPlanOffsetStatus); - new ROS2Callback<>(ros2Node, getTopic(ControllerCrashNotificationPacket.class, robotName), this::acceptControllerCrashNotificationPacket); - new ROS2Callback<>(ros2Node, getTopic(CapturabilityBasedStatus.class, robotName), this::acceptCapturabilityBasedStatus); - new ROS2Callback<>(ros2Node, getTopic(WalkingStatusMessage.class, robotName), this::acceptWalkingStatusMessage); + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), this::acceptRobotConfigurationData); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, HighLevelStateChangeStatusMessage.class), this::acceptHighLevelStateChangeStatusMessage); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, WalkingControllerFailureStatusMessage.class), this::acceptWalkingControllerFailureStatusMessage); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, PlanOffsetStatus.class), this::acceptPlanOffsetStatus); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, ControllerCrashNotificationPacket.class), this::acceptControllerCrashNotificationPacket); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, CapturabilityBasedStatus.class), this::acceptCapturabilityBasedStatus); + new ROS2Callback<>(ros2Node, ControllerAPI.getTopic(controllerOutputTopic, WalkingStatusMessage.class), this::acceptWalkingStatusMessage); } public void registerAbortedListener(Notification abortedListener) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/WalkingFootstepTracker.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/WalkingFootstepTracker.java index e17e8f6ff067..95996093aaa1 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/WalkingFootstepTracker.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/walkingController/WalkingFootstepTracker.java @@ -2,6 +2,8 @@ import controller_msgs.msg.dds.*; import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.packets.ExecutionMode; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; @@ -10,11 +12,11 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2NodeInterface; import us.ihmc.ros2.ROS2Subscription; +import us.ihmc.ros2.ROS2Topic; import java.util.ArrayList; import java.util.List; -import static us.ihmc.communication.HumanoidControllerAPI.getTopic; import static us.ihmc.tools.string.StringTools.format; /** @@ -40,11 +42,12 @@ public class WalkingFootstepTracker public WalkingFootstepTracker(ROS2NodeInterface ros2Node, String robotName) { - footstepDataListSubscriber = ros2Node.createSubscription2(getTopic(FootstepDataListMessage.class, robotName), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + footstepDataListSubscriber = ros2Node.createSubscription2(ControllerAPI.getTopic(controllerOutputTopic, FootstepDataListMessage.class), this::interceptFootstepDataListMessage); - footstepStatusSubscriber = ros2Node.createSubscription2(getTopic(FootstepStatusMessage.class, robotName), + footstepStatusSubscriber = ros2Node.createSubscription2(ControllerAPI.getTopic(controllerOutputTopic, FootstepStatusMessage.class), this::acceptFootstepStatusMessage); - footstepQueueStatusSubscriber = ros2Node.createSubscription2(getTopic(FootstepQueueStatusMessage.class, robotName), + footstepQueueStatusSubscriber = ros2Node.createSubscription2(ControllerAPI.getTopic(controllerOutputTopic, FootstepQueueStatusMessage.class), this::acceptFootstepQueueStatusMessage); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/PlanarRegionMappingHandler.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/PlanarRegionMappingHandler.java index d4771f80937b..f1e66a55b6dc 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/PlanarRegionMappingHandler.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/PlanarRegionMappingHandler.java @@ -11,6 +11,7 @@ import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.StepGeneratorAPIDefinition; import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.packets.PlanarRegionMessageConverter; @@ -34,6 +35,7 @@ import us.ihmc.robotics.geometry.PlanarRegionsList; import us.ihmc.robotics.geometry.FramePlanarRegionsList; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.tools.thread.ExecutorServiceTools; import us.ihmc.tools.thread.MissingThreadTools; import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; @@ -139,7 +141,8 @@ public PlanarRegionMappingHandler(String simpleRobotName, ROS2Node ros2Node) controllerRegionsPublisher = ros2Node.createPublisher(StepGeneratorAPIDefinition.getTopic(PlanarRegionsListMessage.class, simpleRobotName)); ros2Helper.subscribeViaCallback(PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, latestIncomingRegions::set); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, simpleRobotName), message -> + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(simpleRobotName); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, WalkingControllerFailureStatusMessage.class), message -> { setEnableLiveMode(false); resetMap(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index e5d43ad67808..f0495f42a141 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -10,6 +10,7 @@ import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -20,6 +21,7 @@ import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.time.Instant; @@ -57,7 +59,8 @@ public RapidHeightMapManager(OpenCLManager openCLManager, ros2.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); if (robotModel != null) // Will be null on test bench { - ros2.subscribeViaVolatileCallback(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotModel.getSimpleRobotName()), message -> + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotModel.getSimpleRobotName()); + ros2.subscribeViaVolatileCallback(ControllerAPI.getTopic(controllerOutputTopic, HighLevelStateChangeStatusMessage.class), message -> { // Automatically reset the height map when the robot goes into the walking state if (message.getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) resetHeightMapRequested.set(); diff --git a/ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/AbstractBehavior.java b/ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/AbstractBehavior.java index df46b8f0945b..78ef78526e53 100644 --- a/ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/AbstractBehavior.java +++ b/ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/AbstractBehavior.java @@ -108,8 +108,8 @@ public AbstractBehavior(String robotName, String namePrefix, ROS2Node ros2Node) kinematicsPlanningToolboxInputTopic = ToolboxAPIs.KINEMATICS_PLANNING_TOOLBOX.withRobot(robotName).withInput(); kinematicsPlanningToolboxOutputTopic = ToolboxAPIs.KINEMATICS_PLANNING_TOOLBOX.withRobot(robotName).withOutput(); - controllerInputTopic = HumanoidControllerAPI.HUMANOID_CONTROLLER.withRobot(robotName).withInput(); - controllerOutputTopic = HumanoidControllerAPI.HUMANOID_CONTROLLER.withRobot(robotName).withOutput(); + controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName); + controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); behaviorInputTopic = DeprecatedAPIs.BEHAVIOR_MODULE.withRobot(robotName).withInput(); behaviorOutputTopic = DeprecatedAPIs.BEHAVIOR_MODULE.withRobot(robotName).withOutput(); diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/PelvisPoseCorrectionCommunicator.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/PelvisPoseCorrectionCommunicator.java index a7f679e5836f..469c2b8ddb88 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/PelvisPoseCorrectionCommunicator.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/PelvisPoseCorrectionCommunicator.java @@ -5,9 +5,11 @@ import controller_msgs.msg.dds.LocalizationPacket; import controller_msgs.msg.dds.PelvisPoseErrorPacket; import ihmc_common_msgs.msg.dds.StampedPosePacket; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.pubsub.subscriber.Subscriber; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; public class PelvisPoseCorrectionCommunicator implements PelvisPoseCorrectionCommunicatorInterface @@ -20,8 +22,9 @@ public PelvisPoseCorrectionCommunicator(RealtimeROS2Node realtimeROS2Node, Strin { if (realtimeROS2Node != null && robotName != null) { - poseErrorPublisher = realtimeROS2Node.createPublisher(StateEstimatorAPI.getTopic(PelvisPoseErrorPacket.class, robotName)); - localizationPublisher = realtimeROS2Node.createPublisher(StateEstimatorAPI.getTopic(LocalizationPacket.class, robotName)); + ROS2Topic controllerInputTopic = HumanoidControllerAPI.getInputTopic(robotName); + poseErrorPublisher = realtimeROS2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, PelvisPoseErrorPacket.class)); + localizationPublisher = realtimeROS2Node.createPublisher(ControllerAPI.getTopic(controllerInputTopic, LocalizationPacket.class)); } else { diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/headless/LocalizationAndMappingTask.java b/ihmc-perception/src/main/java/us/ihmc/perception/headless/LocalizationAndMappingTask.java index d587b63a2bba..0e2a75d86cdd 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/headless/LocalizationAndMappingTask.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/headless/LocalizationAndMappingTask.java @@ -8,6 +8,7 @@ import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.StepGeneratorAPIDefinition; import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.packets.PlanarRegionMessageConverter; @@ -116,13 +117,14 @@ public LocalizationAndMappingTask(String simpleRobotName, slamOutputRegionsPublisher = ros2Node.createPublisher(PerceptionAPI.SLAM_OUTPUT_RAPID_REGIONS); ros2Helper.subscribeViaCallback(terrainRegionsTopic, this::onPlanarRegionsReceived); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, simpleRobotName), message -> + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(simpleRobotName); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, WalkingControllerFailureStatusMessage.class), message -> { LogTools.warn("Resetting Map (Walking Failure Detected)"); setEnableLiveMode(false); }); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(HighLevelStateMessage.class, simpleRobotName), highLevelState::set); + ros2Helper.subscribeViaCallback(ControllerAPI.getTopic(controllerOutputTopic, HighLevelStateMessage.class), highLevelState::set); updateMapFuture = executorService.scheduleAtFixedRate(this::scheduledUpdate, 0, SCHEDULED_UPDATE_PERIOD_MS, TimeUnit.MILLISECONDS); } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/headless/TerrainPerceptionProcessWithDriver.java b/ihmc-perception/src/main/java/us/ihmc/perception/headless/TerrainPerceptionProcessWithDriver.java index c0b839b93cf4..ac04c7b37372 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/headless/TerrainPerceptionProcessWithDriver.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/headless/TerrainPerceptionProcessWithDriver.java @@ -6,8 +6,9 @@ import org.bytedeco.opencv.opencv_core.Mat; import perception_msgs.msg.dds.ImageMessage; import us.ihmc.commons.thread.Notification; +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.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.referenceFrame.FramePose3D; @@ -152,7 +153,8 @@ public TerrainPerceptionProcessWithDriver(String robotName, depthBytedecoImage = new BytedecoImage(realsense.getDepthWidth(), realsense.getDepthHeight(), opencv_core.CV_16UC1); - ros2Node.createSubscription(StateEstimatorAPI.getRobotConfigurationDataTopic(robotName).withTypeName(RobotConfigurationData.class), s -> + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class), s -> { LogTools.warn("Realsense focal length is 0.0, not publishing data"); }); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java index cade8bc77ac2..49e9f89cf00e 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java @@ -6,6 +6,7 @@ import perception_msgs.msg.dds.LidarScanMessage; import us.ihmc.commons.thread.ThreadTools; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; @@ -18,6 +19,7 @@ import us.ihmc.perception.gpuHeightMap.HeightMapKernel; import us.ihmc.pubsub.subscriber.Subscriber; import us.ihmc.ros2.NewMessageListener; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParametersBasics; @@ -53,7 +55,8 @@ public RemoteHeightMapUpdater(String robotName, Supplier groundF ROS2PublisherBasics heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_OUTPUT); ros2Node.createSubscription(PerceptionAPI.HEIGHT_MAP_STATE_REQUEST, subscriber -> consumeStateRequestMessage(subscriber.readNextData())); - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, robotName), + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(robotName); + ros2Node.createSubscription(ControllerAPI.getTopic(controllerOutputTopic, WalkingStatusMessage.class), subscriber -> consumeWalkingStatusMessage(subscriber.readNextData())); heightMapUpdater = new HeightMapUpdater(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/logging/PerceptionDataLogger.java b/ihmc-perception/src/main/java/us/ihmc/perception/logging/PerceptionDataLogger.java index ff6889fec019..98491dbb99bf 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/logging/PerceptionDataLogger.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/logging/PerceptionDataLogger.java @@ -9,10 +9,8 @@ import org.bytedeco.javacpp.LongPointer; import perception_msgs.msg.dds.ImageMessage; import us.ihmc.commons.Conversions; -import us.ihmc.communication.CommunicationMode; -import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.ROS2Tools; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.*; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.geometry.Pose3D; @@ -26,6 +24,7 @@ import us.ihmc.pubsub.DomainFactory; import us.ihmc.pubsub.common.SampleInfo; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.tools.IHMCCommonPaths; @@ -262,7 +261,8 @@ public void startLogging(String logFileName, String simpleRobotName) // Add callback for Robot Configuration Data if (channels.get(PerceptionLoggerConstants.ROBOT_CONFIGURATION_DATA_NAME).isEnabled()) { - var robotConfigurationDataSubscription = ros2Helper.subscribe(StateEstimatorAPI.getRobotConfigurationDataTopic(simpleRobotName)); + ROS2Topic controllerOutputTopic = HumanoidControllerAPI.getOutputTopic(simpleRobotName); + var robotConfigurationDataSubscription = ros2Helper.subscribe(ControllerAPI.getTopic(controllerOutputTopic, RobotConfigurationData.class)); robotConfigurationDataSubscription.addCallback(this::logRobotConfigurationData); runnablesToStopLogging.addLast(robotConfigurationDataSubscription::destroy); } diff --git a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisher.java b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisher.java index c8cab700666e..cf48afb61c52 100644 --- a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisher.java +++ b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisher.java @@ -6,7 +6,7 @@ import controller_msgs.msg.dds.IMUPacket; import controller_msgs.msg.dds.RobotConfigurationData; import controller_msgs.msg.dds.SpatialVectorMessage; -import us.ihmc.communication.StateEstimatorAPI; +import us.ihmc.communication.controllerAPI.ControllerAPI; import us.ihmc.ros2.ROS2PublisherBasics; import us.ihmc.communication.packets.MessageTools; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -75,7 +75,7 @@ public RobotConfigurationDataPublisher(RealtimeROS2Node realtimeROS2Node, this.publishPeriod = publishPeriod; robotConfigurationData.setJointNameHash(RobotConfigurationDataFactory.calculateJointNameHash(jointSensorData, forceSensorData, imuSensorData)); - robotConfigurationDataPublisher = realtimeROS2Node.createPublisher(StateEstimatorAPI.getRobotConfigurationDataTopic(outputTopic)); + robotConfigurationDataPublisher = realtimeROS2Node.createPublisher(ControllerAPI.getTopic(outputTopic, RobotConfigurationData.class)); // Create RobotFrameDataPublishers here. for (ReferenceFrame frame : frameData)