us.ihmc.ros2.ROS2Input Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of ihmc-ros2-library
IHMC ROS2 Library
The newest version!
package us.ihmc.ros2;

import us.ihmc.commons.thread.TypedNotification;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.atomic.AtomicReference;
import java.util.function.Consumer;

/**
 * An atomic reference to the latest received message through an optional filter.
 *
 * @param  messageType
 */
public class ROS2Input
{
   private final AtomicReference atomicReference;
   private final MessageFilter messageFilter;
   private boolean hasReceivedFirstMessage = false;
   private final ROS2Callback ros2Callback;
   private final TypedNotification messageNotification = new TypedNotification<>();
   private final List> userCallbacks = new ArrayList<>();

   public ROS2Input(ROS2NodeInterface ros2Node, Class messageType, ROS2Topic topic)
   {
      this(ros2Node, messageType, topic.getName(), topic.getQoS());
   }

   public ROS2Input(ROS2NodeInterface ros2Node, ROS2Topic topic)
   {
      this(ros2Node, topic, ROS2TopicNameTools.newMessageInstance(topic.getType()), message -> true);
   }

   public ROS2Input(ROS2NodeInterface ros2Node, Class messageType, ROS2Topic topic, MessageFilter messageFilter)
   {
      this(ros2Node, messageType, topic.getName(), topic.getQoS(), ROS2TopicNameTools.newMessageInstance(messageType), messageFilter);
   }

   public ROS2Input(ROS2NodeInterface ros2Node, Class messageType, String topicName, ROS2QosProfile qosProfile)
   {
      this(ros2Node, messageType, topicName, qosProfile, ROS2TopicNameTools.newMessageInstance(messageType), message -> true);
   }

   public ROS2Input(ROS2NodeInterface ros2Node, Class messageType, ROS2Topic topic, T initialValue, MessageFilter messageFilter)
   {
      this(ros2Node, messageType, topic.getName(), topic.getQoS(), initialValue, messageFilter);
   }

   public ROS2Input(ROS2NodeInterface ros2Node, ROS2Topic topic, T initialValue, MessageFilter messageFilter)
   {
      this(ros2Node, topic.getType(), topic.getName(), topic.getQoS(), initialValue, messageFilter);
   }

   public ROS2Input(ROS2NodeInterface ros2Node, Class messageType, String topicName, ROS2QosProfile qosProfile, T initialValue, MessageFilter messageFilter)
   {
      atomicReference = new AtomicReference<>(initialValue);
      this.messageFilter = messageFilter;
      ros2Callback = new ROS2Callback<>(ros2Node, messageType, topicName, qosProfile, this::messageReceivedCallback);
   }

   public interface MessageFilter
   {
      boolean accept(T message);
   }

   private void messageReceivedCallback(T incomingData)
   {
      if (messageFilter.accept(incomingData))
      {
         atomicReference.set(incomingData);
         messageNotification.set(incomingData);
         for (Consumer userCallback : userCallbacks)
         {
            userCallback.accept(incomingData);
         }
         hasReceivedFirstMessage = true;
      }
   }

   public T getLatest()
   {
      return atomicReference.get();
   }

   public TypedNotification getMessageNotification()
   {
      return messageNotification;
   }

   public boolean hasReceivedFirstMessage()
   {
      return hasReceivedFirstMessage;
   }

   public void addCallback(Consumer messageReceivedCallback)
   {
      userCallbacks.add(messageReceivedCallback);
   }

   public void destroy()
   {
      ros2Callback.destroy();
   }
}