[Feature] CSG Heartbeat

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/CSGROS2CommunicationHelper.java

@@ -3,19 +3,27 @@
 import controller_msgs.msg.dds.ContinuousStepGeneratorInputMessage;
 import controller_msgs.msg.dds.ContinuousStepGeneratorParametersMessage;
 import controller_msgs.msg.dds.ContinuousStepGeneratorStatusMessage;
+import std_msgs.msg.dds.Empty;
 import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters;
 import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
 import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.ContinuousStepGeneratorParametersBasics;
 import us.ihmc.communication.HumanoidControllerAPI;
 import us.ihmc.communication.ROS2Tools;
+import us.ihmc.communication.ros2.ROS2Heartbeat;
 import us.ihmc.ros2.QueuedROS2Subscription;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2Publisher;
+import us.ihmc.ros2.ROS2Topic;
 
 import java.util.function.Consumer;
 
 public class CSGROS2CommunicationHelper
 {
+   public static final ROS2Topic<Empty> CSG_HEARTBEAT_TOPIC = new ROS2Topic<>().withPrefix("ihmc")
+                                                                               .withModule("continuous_step_generator")
+                                                                               .withSuffix("heartbeat")
+                                                                               .withType(Empty.class);
+
    private final ROS2Node ros2Node;
    private final String robotName;
 
@@ -30,6 +38,9 @@ public class CSGROS2CommunicationHelper
    private final ROS2Publisher<ContinuousStepGeneratorInputMessage> csgInputCommandPublisher;
    private final ROS2Publisher<ContinuousStepGeneratorParametersMessage> csgParametersCommandPublisher;
 
+   // Heartbeat to ensure StepGeneratorCommandInputManager stops walking if connection is broken
+   private final ROS2Heartbeat heartbeat;
+
    public CSGROS2CommunicationHelper(String robotName, ROS2Node ros2Node, WalkingControllerParameters walkingControllerParameters)
    {
       this(robotName, ros2Node);
@@ -53,6 +64,9 @@ public CSGROS2CommunicationHelper(String robotName, ROS2Node ros2Node)
 
       csgInputCommandPublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorInputMessage.class, robotName));
       csgParametersCommandPublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorParametersMessage.class, robotName));
+
+      heartbeat = new ROS2Heartbeat(ros2Node, CSG_HEARTBEAT_TOPIC);
+      heartbeat.setAlive(true); // Heartbeat stays alive. The StepGeneratorCommandInputManager will stop if this process dies or Wi-Fi connection is lost
    }
 
    public void publish(ContinuousStepGeneratorInputMessage continuousStepGeneratorInputMessage)
@@ -147,4 +161,9 @@ public ContinuousStepGeneratorStatusMessage getCSGStatusMessage()
    {
       return csgStatusMessage;
    }
+
+   public void destroy()
+   {
+      heartbeat.destroy();
+   }
 }

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java

@@ -12,12 +12,15 @@
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
 import us.ihmc.communication.controllerAPI.command.Command;
+import us.ihmc.communication.ros2.ROS2HeartbeatMonitor;
 import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
 import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeightMapCommand;
 import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
 import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus;
 import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus;
 import us.ihmc.robotics.robotSide.RobotSide;
+import us.ihmc.ros2.ROS2Node;
+import us.ihmc.ros2.ROS2NodeBuilder;
 import us.ihmc.yoVariables.euclid.YoVector2D;
 import us.ihmc.yoVariables.providers.BooleanProvider;
 import us.ihmc.yoVariables.providers.DoubleProvider;
@@ -43,6 +46,7 @@ public class StepGeneratorCommandInputManager implements Updatable
    private final YoDouble turningVelocity;
    private final YoDouble swingHeight;
    private final YoBoolean isUnitVelocities;
+   private final YoBoolean overrideHeartbeat;
    private int ticksToUpdateTheEnvironment = Integer.MAX_VALUE;
    private HighLevelControllerName currentController;
    private ContinuousStepGenerator continuousStepGenerator;
@@ -56,6 +60,11 @@ public class StepGeneratorCommandInputManager implements Updatable
    private final AtomicBoolean shouldSubmitNewRegions = new AtomicBoolean(true);
    private final AtomicInteger ticksSinceUpdatingTheEnvironment = new AtomicInteger(0);
 
+   // ROS 2 stuff
+   // TODO: Destroy these objects properly
+   private final ROS2Node ros2Node;
+   private final ROS2HeartbeatMonitor heartbeatMonitor;
+
    public StepGeneratorCommandInputManager()
    {
       String suffix = "StepGeneratorCommandInputManager";
@@ -65,9 +74,14 @@ public StepGeneratorCommandInputManager()
       swingHeight = new YoDouble("desiredSwingHeight_" + suffix, registry);
       isUnitVelocities = new YoBoolean("isUnitVelocities_" + suffix, registry);
       isUnitVelocities.set(false);
+      overrideHeartbeat = new YoBoolean("overrideHeartbeat_" + suffix, registry);
+      overrideHeartbeat.set(false);
 
       // by default, command input manager is not enabled, set enabled only if #update is called
       commandInputManager.setEnabled(false);
+
+      ros2Node = new ROS2NodeBuilder().build(getClass().getSimpleName().toLowerCase() + "Node");
+      heartbeatMonitor = new ROS2HeartbeatMonitor(ros2Node, CSGROS2CommunicationHelper.CSG_HEARTBEAT_TOPIC);
    }
 
    public void setCSG(ContinuousStepGenerator continuousStepGenerator)
@@ -138,7 +152,15 @@ public void update(double time)
       isOpen = currentController == HighLevelControllerName.WALKING || currentController == HighLevelControllerName.QUICKSTER;
       commandInputManager.setEnabled(isOpen);
 
-      if (commandInputManager.isNewCommandAvailable(ContinuousStepGeneratorInputCommand.class))
+      if (!overrideHeartbeat.getValue() && !heartbeatMonitor.isAlive())
+      {
+         desiredVelocity.setX(0.0);
+         desiredVelocity.setY(0.0);
+         turningVelocity.set(0.0);
+         isUnitVelocities.set(false);
+         walk.set(false);
+      }
+      else if (commandInputManager.isNewCommandAvailable(ContinuousStepGeneratorInputCommand.class))
       {
          ContinuousStepGeneratorInputCommand command = commandInputManager.pollNewestCommand(ContinuousStepGeneratorInputCommand.class);
          desiredVelocity.setX(command.getForwardVelocity());

ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java

@@ -4,12 +4,11 @@
 import us.ihmc.commons.exception.DefaultExceptionHandler;
 import us.ihmc.commons.exception.ExceptionTools;
 import us.ihmc.commons.thread.ThreadTools;
-import us.ihmc.commons.thread.TypedNotification;
-import us.ihmc.ros2.ROS2Input;
+import us.ihmc.commons.thread.Throttler;
+import us.ihmc.pubsub.subscriber.Subscriber;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.tools.Timer;
-import us.ihmc.commons.thread.Throttler;
 
 import java.util.function.Consumer;
 
@@ -28,24 +27,25 @@ public class ROS2HeartbeatMonitor
     */
    public static final double HEARTBEAT_EXPIRATION = 1.25 * ROS2Heartbeat.HEARTBEAT_PERIOD;
    private final Timer timer = new Timer();
+   private final Empty heartbeatMessage = new Empty();
 
    // To provide callback when aliveness changes
    private volatile boolean running = true;
    private final Throttler throttler = new Throttler();
    private boolean wasAlive = false;
    private Consumer<Boolean> callback = null;
-   private final TypedNotification<Boolean> alivenessChangedNotification = new TypedNotification<>();
 
    public ROS2HeartbeatMonitor(ROS2Node ros2Node, ROS2Topic<Empty> heartbeatTopic)
    {
-      ros2Node.createSubscription2(heartbeatTopic, this::receivedHeartbeat);
+      ros2Node.createSubscription(heartbeatTopic, this::receivedHeartbeat);
       ThreadTools.startAsDaemon(() -> ExceptionTools.handle(this::monitorThread, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE),
                                 "HeartbeatMonitor");
    }
 
-   private synchronized void receivedHeartbeat(Empty empty)
+   private synchronized void receivedHeartbeat(Subscriber<Empty> subscriber)
    {
-      timer.reset();
+      if (subscriber.takeNextData(heartbeatMessage, null))
+         timer.reset();
    }
 
    /**
@@ -77,16 +77,10 @@ private void monitorThread()
 
             if (callback != null)
                callback.accept(isAlive);
-            alivenessChangedNotification.set(isAlive);
          }
       }
    }
 
-   public TypedNotification<Boolean> getAlivenessChangedNotification()
-   {
-      return alivenessChangedNotification;
-   }
-
    public void destroy()
    {
       running = false;

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/csg/RDXCSGPanel.java

@@ -4,21 +4,17 @@
 import controller_msgs.msg.dds.ContinuousStepGeneratorParametersMessage;
 import controller_msgs.msg.dds.ContinuousStepGeneratorStatusMessage;
 import imgui.ImGui;
-import imgui.flag.ImGuiMouseButton;
 import imgui.type.ImBoolean;
 import imgui.type.ImDouble;
 import us.ihmc.avatar.drcRobot.DRCRobotModel;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.CSGROS2CommunicationHelper;
-import us.ihmc.commons.thread.Throttler;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
 import us.ihmc.rdx.imgui.RDXPanel;
 import us.ihmc.ros2.ROS2Node;
 
 public class RDXCSGPanel extends RDXPanel
 {
-   private static final double THROTTLER_THREAD_HERTZ = 11.0;
-
    private static final boolean PUBLISH_IN_VELOCITY_UNITS = true;
 
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
@@ -53,26 +49,16 @@ public class RDXCSGPanel extends RDXPanel
    private boolean resetDesiredLateralVelocityNextTick = false;
    private boolean resetDesiredTurningVelocityNextTick = false;
 
-   // Throttler for ensuring we aren't publishing too fast/often
-   private final Throttler csgROS2PublisherThrottler;
-
    public RDXCSGPanel(DRCRobotModel robotModel, ROS2Node ros2Node, boolean createXboxPlugin)
-   {
-      this(new CSGROS2CommunicationHelper(robotModel.getSimpleRobotName(), ros2Node, robotModel.getWalkingControllerParameters()), createXboxPlugin);
-   }
-
-   public RDXCSGPanel(CSGROS2CommunicationHelper communicationHelper, boolean createXboxPlugin)
    {
       super("CSG Controls");
       super.setRenderMethod(this::renderImGuiWidgets);
 
-      this.communicationHelper = communicationHelper;
+      communicationHelper = new CSGROS2CommunicationHelper(robotModel.getSimpleRobotName(), ros2Node, robotModel.getWalkingControllerParameters());
 
       csgParametersCommand = communicationHelper.getCSGParametersCommand();
       csgStatusMessage = communicationHelper.getCSGStatusMessage();
 
-      csgROS2PublisherThrottler = new Throttler().setFrequency(THROTTLER_THREAD_HERTZ);
-
       if (createXboxPlugin)
          xBoxOneCSGPlugin = new RDXXboxOneCSGPlugin(communicationHelper);
    }
@@ -87,7 +73,6 @@ public void update()
    public void renderImGuiWidgets()
    {
       boolean publishCSGInputCommand = false;
-      boolean publishCSGParametersCommand = false;
 
       reset(csgStatusMessage);
 
@@ -208,7 +193,7 @@ public void renderImGuiWidgets()
          csgParametersCommand.setSwingHeight(swingHeight.get());
 
       publishCSGInputCommand = publishCSGInputCommand || requestCSGWalkingChanged || desiredForwardVelocityChanged || desiredLateralVelocityChanged || desiredTurningVelocityChanged;
-      publishCSGParametersCommand = swingDurationChanged || transferDurationChanged || maxStepLengthForwardsChanged || maxStepLengthBackwardsChanged || maxStepWidthChanged || swingHeightChanged;
+      boolean publishCSGParametersCommand = swingDurationChanged || transferDurationChanged || maxStepLengthForwardsChanged || maxStepLengthBackwardsChanged || maxStepWidthChanged || swingHeightChanged;
 
       if (publishCSGInputCommand)
          communicationHelper.publish(csgInputCommand);
@@ -238,5 +223,7 @@ public void destroy()
    {
       if (xBoxOneCSGPlugin != null)
          xBoxOneCSGPlugin.shutDownXboxJoystick();
+
+      communicationHelper.destroy();
    }
 }