Simplify ROS2Heartbeat (#653)

Author: ds58

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

@@ -2,10 +2,7 @@
 
 import std_msgs.msg.dds.Empty;
 import us.ihmc.commons.UnitConversions;
-import us.ihmc.commons.exception.DefaultExceptionHandler;
-import us.ihmc.commons.exception.ExceptionTools;
-import us.ihmc.commons.thread.ThreadTools;
-import us.ihmc.commons.thread.Throttler;
+import us.ihmc.commons.thread.RepeatingTaskThread;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2Publisher;
 import us.ihmc.ros2.ROS2Topic;
@@ -42,23 +39,16 @@ public class ROS2Heartbeat
     */
    public static final double STATUS_FREQUENCY = 2.5;
    public static final double HEARTBEAT_PERIOD = UnitConversions.hertzToSeconds(STATUS_FREQUENCY);
-   private ROS2PublishSubscribeAPI ros2;
-   private ROS2Publisher<Empty> heartbeatPublisher;
-   private final Empty emptyMessage = new Empty();
-   private final ROS2Topic<Empty> heartbeatTopic;
-   private volatile boolean alive = false;
-   private final Throttler throttler = new Throttler();
+   private static final Empty EMPTY_MESSAGE = new Empty();
 
-   public ROS2Heartbeat(ROS2PublishSubscribeAPI ros2, ROS2Topic<Empty> heartbeatTopic)
-   {
-      this.ros2 = ros2;
-      this.heartbeatTopic = heartbeatTopic;
-   }
+   private final ROS2Publisher<Empty> heartbeatPublisher;
+   private final RepeatingTaskThread publishThread;
 
    public ROS2Heartbeat(ROS2Node ros2Node, ROS2Topic<Empty> heartbeatTopic)
    {
-      this.heartbeatTopic = heartbeatTopic;
       heartbeatPublisher = ros2Node.createPublisher(heartbeatTopic);
+      publishThread = new RepeatingTaskThread("Heartbeat-" + heartbeatTopic.getName(), () -> heartbeatPublisher.publish(EMPTY_MESSAGE));
+      publishThread.setFrequencyLimit(STATUS_FREQUENCY);
    }
 
    /**
@@ -67,34 +57,14 @@ public ROS2Heartbeat(ROS2Node ros2Node, ROS2Topic<Empty> heartbeatTopic)
    public void setAlive(boolean alive)
    {
       if (alive)
-      {
-         if (!this.alive)
-            ThreadTools.startAsDaemon(() -> ExceptionTools.handle(this::publishThread, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE),
-                                      "Heartbeat-" + heartbeatTopic.getName());
-         this.alive = true;
-      }
+         publishThread.startRepeating();
       else
-      {
-         this.alive = false;
-      }
-   }
-
-   private void publishThread()
-   {
-      while (alive)
-      {
-         if (ros2 != null)
-            ros2.publish(heartbeatTopic);
-         else
-            heartbeatPublisher.publish(emptyMessage);
-         throttler.waitAndRun(HEARTBEAT_PERIOD);
-      }
+         publishThread.stopRepeating();
    }
 
    public void destroy()
    {
-      setAlive(false);
-      if (heartbeatPublisher != null)
-         heartbeatPublisher.remove();
+      publishThread.kill();
+      heartbeatPublisher.remove();
    }
 }

ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/RemoteFootstepPlannerUI.java

@@ -4,10 +4,11 @@
 import javafx.stage.Stage;
 import us.ihmc.communication.PerceptionAPI;
 import us.ihmc.communication.ros2.ROS2Heartbeat;
-import us.ihmc.communication.ros2.ROS2Helper;
 import us.ihmc.footstepPlanning.communication.FootstepPlannerMessagerAPI;
 import us.ihmc.javaFXToolkit.ApplicationNoModule;
 import us.ihmc.messager.javafx.SharedMemoryJavaFXMessager;
+import us.ihmc.ros2.ROS2Node;
+import us.ihmc.ros2.ROS2NodeBuilder;
 
 /**
  * This class provides a visualizer for the remote footstep planner found in the footstep planner toolbox.
@@ -20,6 +21,7 @@ public class RemoteFootstepPlannerUI extends ApplicationNoModule
    private RemoteUIMessageConverter messageConverter;
 
    private FootstepPlannerUI ui;
+   private final ROS2Node ros2Node = new ROS2NodeBuilder().build("height_map_heartbeat_node");
    private ROS2Heartbeat heightMapHeartbeat;
 
    @Override
@@ -28,7 +30,8 @@ public void start(Stage primaryStage) throws Exception
       messager = new SharedMemoryJavaFXMessager(FootstepPlannerMessagerAPI.API);
       messageConverter = RemoteUIMessageConverter.createConverter(messager, "");
 
-      heightMapHeartbeat = new ROS2Heartbeat(new ROS2Helper("height_map_heartbeat"), PerceptionAPI.REQUEST_HEIGHT_MAP);
+
+      heightMapHeartbeat = new ROS2Heartbeat(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP);
       heightMapHeartbeat.setAlive(true);
 
       messager.startMessager();
@@ -41,6 +44,7 @@ public void start(Stage primaryStage) throws Exception
    public void stop() throws Exception
    {
       heightMapHeartbeat.destroy();
+      ros2Node.destroy();
 
       super.stop();
 

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXYOLOv8Settings.java

@@ -5,15 +5,11 @@
 import imgui.type.ImInt;
 import perception_msgs.msg.dds.YOLOv8ParametersMessage;
 import us.ihmc.commons.thread.Notification;
+import us.ihmc.commons.thread.Throttler;
 import us.ihmc.communication.PerceptionAPI;
 import us.ihmc.communication.ros2.ROS2Heartbeat;
-import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI;
-import us.ihmc.rdx.imgui.ImGuiTools;
+import us.ihmc.communication.ros2.ROS2Helper;
 import us.ihmc.rdx.ui.graphics.RDXVisualizer;
-import us.ihmc.commons.thread.Throttler;
-
-import java.util.Arrays;
-import java.util.HashSet;
 
 /*
  *  FIXME: It doesn't make sense to have a visualizer for settings.
@@ -25,7 +21,7 @@ public class RDXYOLOv8Settings extends RDXVisualizer
    private static final String[] AVAILABLE_SENSORS = {"ZED", "D455"};
    private static final double MESSAGE_PUBLISH_PERIOD = 2; // publish messages every 2 seconds
 
-   private final ROS2PublishSubscribeAPI ros2;
+   private ROS2Helper ros2Helper;
 
    private final ImFloat confidenceThreshold = new ImFloat(0.8f);
    private final ImFloat nmsThreshold = new ImFloat(0.1f);
@@ -41,14 +37,14 @@ public class RDXYOLOv8Settings extends RDXVisualizer
    private final Throttler messagePublishThrottler = new Throttler().setPeriod(MESSAGE_PUBLISH_PERIOD);
    private final Notification parametersChanged = new Notification();
 
-   public RDXYOLOv8Settings(String title, ROS2PublishSubscribeAPI ros2)
+   public RDXYOLOv8Settings(String title, ROS2Helper ros2Helper)
    {
       super(title);
 
-      this.ros2 = ros2;
+      this.ros2Helper = ros2Helper;
 
-      demandYOLOv8ICPZed = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_YOLO_ZED);
-      demandYOLOv8ICPRealsense = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_YOLO_REALSENSE);
+      demandYOLOv8ICPZed = new ROS2Heartbeat(ros2Helper.getROS2Node(), PerceptionAPI.REQUEST_YOLO_ZED);
+      demandYOLOv8ICPRealsense = new ROS2Heartbeat(ros2Helper.getROS2Node(), PerceptionAPI.REQUEST_YOLO_REALSENSE);
 
       // Select all target detections at beginning
 //      targetDetections.addAll(Arrays.asList(YOLOv8DetectionClass.values()));
@@ -122,7 +118,7 @@ public void update()
 //         for (YOLOv8DetectionClass targetDetection : targetDetections)
 //            message.getTargetDetectionClasses().add(targetDetection.toByte());
 
-         ros2.publish(PerceptionAPI.YOLO_PARAMETERS, message);
+         ros2Helper.publish(PerceptionAPI.YOLO_PARAMETERS, message);
       }
    }