us.ihmc.ros2.ROS2NodeInterface Maven / Gradle / Ivy
The newest version!
package us.ihmc.ros2;
import com.eprosima.xmlschemas.fastrtps_profiles.TransportDescriptorType;
import com.eprosima.xmlschemas.fastrtps_profiles.TransportDescriptorType.InterfaceWhiteList;
import jakarta.xml.bind.JAXBElement;
import us.ihmc.log.LogTools;
import us.ihmc.pubsub.TopicDataType;
import us.ihmc.pubsub.attributes.ParticipantProfile;
import us.ihmc.pubsub.attributes.PublisherAttributes;
import us.ihmc.pubsub.attributes.SubscriberAttributes;
import us.ihmc.pubsub.common.MatchingInfo;
import us.ihmc.pubsub.common.Time;
import us.ihmc.pubsub.impl.fastRTPS.FastRTPSDomain;
import us.ihmc.pubsub.subscriber.Subscriber;
import javax.xml.namespace.QName;
import java.net.InetAddress;
import java.util.UUID;
import java.util.function.Consumer;
public interface ROS2NodeInterface
{
int DEFAULT_QUEUE_SIZE = 10;
static ParticipantProfile createParticipantAttributes(int domainId, boolean useSharedMemory, InetAddress... addressRestriction)
{
ParticipantProfile participantAttributes = ParticipantProfile.create().domainId(domainId).discoveryLeaseDuration(Time.Infinite);
// Always override the transport so we're sure what it is
participantAttributes.useBuiltinTransports(false);
// If this is false then shared memory will be disabled
if (useSharedMemory)
{
participantAttributes.addSharedMemoryTransport();
}
// Add custom UDPv4 transport
String transportName = UUID.randomUUID().toString();
TransportDescriptorType transportDescriptor = new TransportDescriptorType();
transportDescriptor.setTransportId(transportName);
transportDescriptor.setType("UDPv4");
// Apply address restrictions
// Check for null on the first element, to make sure passing in null works as usual -> no address restrictions
if (addressRestriction != null && addressRestriction.length > 0 && addressRestriction[0] != null)
{
TransportDescriptorType.InterfaceWhiteList addressWhitelist = new InterfaceWhiteList();
for (InetAddress addr : addressRestriction)
{
JAXBElement addressElement = new JAXBElement<>(new QName(FastRTPSDomain.FAST_DDS_XML_NAMESPACE, "address"), String.class, addr.getHostAddress());
addressWhitelist.getAddressOrInterface().add(addressElement);
}
transportDescriptor.setInterfaceWhiteList(addressWhitelist);
}
participantAttributes.addTransport(transportDescriptor);
return participantAttributes;
}
static boolean useSHMFromEnvironment()
{
String disableSharedMemoryTransportEnv = System.getenv("ROS_DISABLE_SHARED_MEMORY_TRANSPORT");
if (disableSharedMemoryTransportEnv == null)
{
return false;
}
else if (disableSharedMemoryTransportEnv.equalsIgnoreCase("true"))
{
LogTools.info("Shared memory transport is disabled via environment variable ROS_DISABLE_SHARED_MEMORY_TRANSPORT");
return false;
}
else
{
return true;
}
}
static int domainFromEnvironment()
{
String rosDomainId = System.getenv("ROS_DOMAIN_ID");
int rosDomainIdAsInteger = 0; // default to 0
if (rosDomainId != null)
{
rosDomainId = rosDomainId.trim();
try
{
rosDomainIdAsInteger = Integer.parseInt(rosDomainId);
}
catch (NumberFormatException e)
{
LogTools.warn("Environment variable ROS_DOMAIN_ID cannot be parsed as an integer: {}", rosDomainId);
}
}
LogTools.info("ROS_DOMAIN_ID environment variable is {}.", rosDomainIdAsInteger);
LogTools.info("Nodes created without a specified domain ID will use ROS_DOMAIN_ID.");
return rosDomainIdAsInteger;
}
/**
* Create a new ROS 2 compatible publisher in this node.
*
* @param topic topic
* @return a ROS 2 publisher
*/
default ROS2PublisherBasics createPublisher(ROS2Topic topic)
{
return createPublisher(topic.getType(), topic.getName(), topic.getQoS());
}
/**
* Create a new ROS 2 compatible publisher in this node.
* This call makes a publisher with the default settings.
*
* @param topicDataType The topic data type of the message
* @param topicName Name for the topic
* @return a ROS 2 publisher
*/
default ROS2PublisherBasics createPublisher(TopicDataType topicDataType, String topicName)
{
return createPublisher(topicDataType, topicName, ROS2QosProfile.DEFAULT());
}
/**
* Create a new ROS 2 compatible publisher in this node.
*
* @param messageType The type of the message
* @param topicName Name for the topic
* @return a ROS 2 publisher
*/
default ROS2PublisherBasics createPublisher(Class messageType, String topicName)
{
return createPublisher(messageType, topicName, ROS2QosProfile.DEFAULT());
}
/**
* Create a new ROS 2 compatible publisher in this node.
*
* @param messageType The type of the message
* @param topicName Name for the topic
* @param qosProfile ROS 2 qos profile
* @return a ROS 2 publisher
*/
default ROS2PublisherBasics createPublisher(Class messageType, String topicName, ROS2QosProfile qosProfile)
{
TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType);
return createPublisher(topicDataType, createPublisherAttributes(topicDataType, topicName, qosProfile));
}
/**
* Create a new ROS 2 compatible publisher in this node.
*
* @param topicDataType The topic data type of the message
* @param topicName Name for the topic
* @param qosProfile ROS 2 qos profile
* @return a ROS 2 publisher
*/
default ROS2PublisherBasics createPublisher(TopicDataType topicDataType, String topicName, ROS2QosProfile qosProfile)
{
return createPublisher(topicDataType, createPublisherAttributes(topicDataType, topicName, qosProfile));
}
/**
* Create publisher attributes for a topic.
*
* @param Data type of the topic
* @param topicDataType Data type serializer of the topic
* @param topicName Topic Name
* @param qosProfile Initial ROS 2 qos profile
* @return PublisherAttributes for createPublisher
*/
PublisherAttributes createPublisherAttributes(TopicDataType topicDataType, String topicName, ROS2QosProfile qosProfile);
/**
* Create a new ROS 2 compatible publisher in this node.
*
* @param topicDataType The topic data type of the message
* @param publisherAttributes Publisher attributes created with @see{createPublisherAttributes}
* @return a ROS 2 publisher
*/
ROS2PublisherBasics createPublisher(TopicDataType topicDataType, PublisherAttributes publisherAttributes);
/**
* Create a new realtime subscription with default qos profile and queue depth. Incoming messages
* are stored in a queue of depth queueSize and can be polled by the realtime thread. This function
* will allocate a queue of depth 10.
*
* @param topicDataType Data type to subscribe to
* @param topicName Topic name
* @return A realtime-safe ROS 2 subscriber
*/
default QueuedROS2Subscription createQueuedSubscription(TopicDataType topicDataType, String topicName)
{
return createQueuedSubscription(topicDataType, topicName, ROS2QosProfile.DEFAULT(), DEFAULT_QUEUE_SIZE);
}
/**
* Create a new realtime subscription. Incoming messages are stored in a queue of depth queueSize
* and can be polled by the realtime thread. The queueSize should weigh memory requirements of the
* message vs the chance to lose incoming messages because the queue is full.
*
* @param topic topic
* @return a realtime-safe ROS 2 subscriber
*/
default QueuedROS2Subscription createQueuedSubscription(ROS2Topic topic)
{
return createQueuedSubscription(topic, DEFAULT_QUEUE_SIZE);
}
/**
* Create a new realtime subscription. Incoming messages are stored in a queue of depth queueSize
* and can be polled by the realtime thread. The queueSize should weigh memory requirements of the
* message vs the chance to lose incoming messages because the queue is full.
*
* @param topic topic
* @param queueSize Depth of the subscription queue (10 would be a good size for small messages)
* @return a realtime-safe ROS 2 subscriber
*/
default QueuedROS2Subscription createQueuedSubscription(ROS2Topic topic, int queueSize)
{
return createQueuedSubscription(topic.getType(), topic.getName(), topic.getQoS(), queueSize);
}
/**
* Create a new realtime subscription. Incoming messages are stored in a queue of depth queueSize
* and can be polled by the realtime thread. The queueSize should weigh memory requirements of the
* message vs the chance to lose incoming messages because the queue is full.
*
* @param messageType The type of the message
* @param topicName Topic name
* @param qosProfile Desired ros qos profile
* @param queueSize Depth of the subscription queue (10 would be a good size for small messages)
* @return a realtime-safe ROS 2 subscriber
*/
default QueuedROS2Subscription createQueuedSubscription(Class messageType, String topicName, ROS2QosProfile qosProfile, int queueSize)
{
TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType);
return createQueuedSubscription(topicDataType, createSubscriberAttributes(topicName, topicDataType, qosProfile), queueSize);
}
/**
* Create a new realtime subscription. Incoming messages are stored in a queue of depth queueSize
* and can be polled by the realtime thread. The queueSize should weigh memory requirements of the
* message vs the chance to lose incoming messages because the queue is full.
*
* @param topicDataType Data type to subscribe to
* @param topicName Topic name
* @param qosProfile Desired ros qos profile
* @param queueSize Depth of the subscription queue (10 would be a good size for small messages)
* @return a realtime-safe ROS 2 subscriber
*/
default QueuedROS2Subscription createQueuedSubscription(TopicDataType topicDataType, String topicName, ROS2QosProfile qosProfile, int queueSize)
{
return createQueuedSubscription(topicDataType, createSubscriberAttributes(topicName, topicDataType, qosProfile), queueSize);
}
/**
* Create a new realtime subscription. Incoming messages are stored in a queue of depth queueSize
* and can be polled by the realtime thread. The queueSize should weigh memory requirements of the
* message vs the chance to lose incoming messages because the queue is full.
*
* @param topicDataType Data type to subscribe to
* @param subscriberAttributes Attributes for this topic, created
* using @see{createSubscriberAttributes}
* @param queueSize Depth of the subscription queue (10 would be a good size for small
* messages)
* @return a realtime-safe ROS 2 subscriber
*/
QueuedROS2Subscription createQueuedSubscription(TopicDataType topicDataType, SubscriberAttributes subscriberAttributes, int queueSize);
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param topicDataType The topic data type of the message
* @param newMessageListener New message listener
* @param topicName Name for the topic
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(TopicDataType topicDataType, NewMessageListener newMessageListener, String topicName)
{
return createSubscription(topicDataType, newMessageListener, topicName, ROS2QosProfile.DEFAULT());
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param topicDataType The topic data type of the message
* @param newMessageListener New message listener
* @param topicName Name for the topic
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(TopicDataType topicDataType,
NewMessageListener newMessageListener,
String topicName,
ROS2QosProfile qosProfile)
{
return createSubscription(topicDataType, newMessageListener, createSubscriberAttributes(topicName, topicDataType, qosProfile));
}
/**
* Create a new ROS 2 compatible subscription.
*
* @param topicDataType The topic data type of the message
* @param subscriberListener Listener for new messages
* @param subscriberAttributes Attributes for this topic, created
* using @see{createSubscriberAttributes}
*/
ROS2Subscription createSubscription(TopicDataType topicDataType,
NewMessageListener subscriberListener,
SubscriberAttributes subscriberAttributes);
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* Note: This method generates garbage!
*
* @param topic The topic
* @param messageCallback Message listener that gives the taken message directly
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription2(ROS2Topic topic, Consumer messageCallback)
{
TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(topic.getType());
return createSubscription(topicDataType, new NewMessageListener()
{
@Override
public void onNewDataMessage(Subscriber subscriber)
{
T incomingData = subscriber.takeNextData();
if (incomingData != null)
{
messageCallback.accept(incomingData);
}
else
{
LogTools.warn("Received null from takeNextData()");
}
}
@Override
public void onSubscriptionMatched(Subscriber subscriber, MatchingInfo info)
{
// Do nothing
}
}, createSubscriberAttributes(topic.getName(), topicDataType, topic.getQoS()));
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param topic The topic
* @param newMessageListener New message listener
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(ROS2Topic topic,
NewMessageListener newMessageListener)
{
return createSubscription(topic.getType(), newMessageListener, topic.getName(), topic.getQoS());
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param topic The topic
* @param newMessageListener New message listener
* @param subscriptionMatchedListener Subscription matched listener
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(ROS2Topic topic,
NewMessageListener newMessageListener,
SubscriptionMatchedListener subscriptionMatchedListener)
{
return createSubscription(topic.getType(), newMessageListener, subscriptionMatchedListener, topic.getName(), topic.getQoS());
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param messageType The type of the message
* @param newMessageListener New message listener
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(Class messageType,
NewMessageListener newMessageListener,
String topicName)
{
return createSubscription(messageType, newMessageListener, topicName, ROS2QosProfile.DEFAULT());
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param messageType The type of the message
* @param newMessageListener New message listener
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(Class messageType,
NewMessageListener newMessageListener,
String topicName,
ROS2QosProfile qosProfile)
{
TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType);
return createSubscription(topicDataType, newMessageListener, topicName, qosProfile);
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param messageType The type of the message
* @param newMessageListener New message listener
* @param subscriptionMatchedListener Subscription matched listener
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(Class messageType,
NewMessageListener newMessageListener,
SubscriptionMatchedListener subscriptionMatchedListener,
String topicName,
ROS2QosProfile qosProfile)
{
TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType);
return createSubscription(topicDataType, newMessageListener, subscriptionMatchedListener, topicName, qosProfile);
}
/**
* Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the
* default qos profile.
*
* @param topicDataType The topic data type of the message
* @param newMessageListener New message listener
* @param subscriptionMatchedListener Subscription matched listener
* @param topicName Name for the topic
* @param qosProfile ROS 2 qos Profile
* @return a ROS 2 subscription
*/
default ROS2Subscription createSubscription(TopicDataType topicDataType,
NewMessageListener newMessageListener,
SubscriptionMatchedListener subscriptionMatchedListener,
String topicName,
ROS2QosProfile qosProfile)
{
return createSubscription(topicDataType, new NewMessageListener()
{
@Override
public void onNewDataMessage(Subscriber subscriber)
{
newMessageListener.onNewDataMessage(subscriber);
}
@Override
public void onSubscriptionMatched(Subscriber subscriber, MatchingInfo info)
{
subscriptionMatchedListener.onSubscriptionMatched(subscriber, info);
}
}, createSubscriberAttributes(topicName, topicDataType, qosProfile));
}
String getName();
String getNamespace();
SubscriberAttributes createSubscriberAttributes(String topicName, TopicDataType topicDataType, ROS2QosProfile qosProfile);
}