Undeprecate and create old kinematics simulation more easily.

Author: calvertdw

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java

@@ -96,7 +96,8 @@
 import java.util.concurrent.atomic.AtomicReference;
 
 /**
- * @deprecated Use {@link SCS2AvatarSimulationFactory#setKinematicsSimulation} instead.
+ * We built {@link SCS2AvatarSimulationFactory#setKinematicsSimulation} to replace this,
+ * but apparently this one still works better for some reason.
  */
 public class HumanoidKinematicsSimulation
 {
@@ -160,7 +161,10 @@ private HumanoidKinematicsSimulation(DRCRobotModel robotModel, HumanoidKinematic
       this.kinematicsSimulationParameters = kinematicsSimulationParameters;
 
       // instantiate some existing controller ROS2 API?
-      ros2Node = new ROS2NodeBuilder().build(HumanoidControllerAPI.HUMANOID_KINEMATICS_CONTROLLER_NODE_NAME);
+      ROS2NodeBuilder nodeBuilder = kinematicsSimulationParameters.getRos2NodeBuilder();
+      if (nodeBuilder == null)
+         nodeBuilder = new ROS2NodeBuilder();
+      ros2Node = nodeBuilder.build(HumanoidControllerAPI.HUMANOID_KINEMATICS_CONTROLLER_NODE_NAME);
       heartbeat = new ROS2Heartbeat(ros2Node, KINEMATICS_SIMULATION_HEARTBEAT);
 
       String robotName = robotModel.getSimpleRobotName();
@@ -261,7 +265,7 @@ private HumanoidKinematicsSimulation(DRCRobotModel robotModel, HumanoidKinematic
       walkingParentRegistry.addChild(walkingController.getYoVariableRegistry());
 
       // create controller network subscriber here!!
-      realtimeROS2Node = new ROS2NodeBuilder().buildRealtime(HumanoidControllerAPI.HUMANOID_KINEMATICS_CONTROLLER_NODE_NAME + "_rt");
+      realtimeROS2Node = nodeBuilder.buildRealtime(HumanoidControllerAPI.HUMANOID_KINEMATICS_CONTROLLER_NODE_NAME + "_rt");
       ROS2Topic inputTopic = HumanoidControllerAPI.getInputTopic(robotName);
       ROS2Topic outputTopic = HumanoidControllerAPI.getOutputTopic(robotName);
       ControllerNetworkSubscriber controllerNetworkSubscriber = new ControllerNetworkSubscriber(inputTopic,

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulationParameters.java

@@ -1,10 +1,10 @@
 package us.ihmc.avatar.kinematicsSimulation;
 
-import us.ihmc.avatar.scs2.SCS2AvatarSimulationFactory;
 import us.ihmc.commons.UnitConversions;
+import us.ihmc.ros2.ROS2NodeBuilder;
 
 /**
- * @deprecated Use {@link SCS2AvatarSimulationFactory#setKinematicsSimulation} instead.
+ * To be used with {@link HumanoidKinematicsSimulation}.
  */
 public class HumanoidKinematicsSimulationParameters
 {
@@ -20,6 +20,7 @@ public class HumanoidKinematicsSimulationParameters
    private double dt = UnitConversions.hertzToSeconds(70);
    private boolean runNoFasterThanMaxRealtimeRate = true;
    private double maxRealtimeRate = 2.0;
+   private ROS2NodeBuilder ros2NodeBuilder = null;
 
    public double getInitialGroundHeight()
    {
@@ -60,7 +61,7 @@ public void setInitialRobotY(double initialRobotY)
    {
       this.initialRobotY = initialRobotY;
    }
-   
+
    public double getInitialRobotZ()
    {
       return initialRobotZ;
@@ -120,7 +121,7 @@ public boolean runNoFasterThanMaxRealtimeRate()
    {
       return runNoFasterThanMaxRealtimeRate;
    }
-   
+
    public void setMaxRealtimeRate(double maxRealtimeRate)
    {
       this.maxRealtimeRate = maxRealtimeRate;
@@ -140,4 +141,14 @@ public void setEnablePeriodicThread(boolean createPeriodicThread)
    {
       this.createPeriodicThread = createPeriodicThread;
    }
+
+   public void setRos2NodeBuilder(ROS2NodeBuilder ros2NodeBuilder)
+   {
+      this.ros2NodeBuilder = ros2NodeBuilder;
+   }
+
+   public ROS2NodeBuilder getRos2NodeBuilder()
+   {
+      return ros2NodeBuilder;
+   }
 }