def __init__(self, topic, client_id, callback, node_handle, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
client_id -- the ID of the client subscribing
callback -- this client's callback, that will be called for incoming
messages
node_handle -- Handle to a rclpy node to create the publisher.
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topics_names_and_types = dict(node_handle.get_topic_names_and_types())
topic_type = topics_names_and_types.get(topic)
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# topic_type is a list of types or None at this point; only one type is supported.
if topic_type is not None:
if len(topic_type) > 1:
node_handle.get_logger().warning(
'More than one topic type detected: {}'.format(topic_type))
topic_type = topic_type[0]
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
msg_type_string = msg_class_type_repr(msg_class)
if topic_type is not None and topic_type != msg_type_string:
raise TypeConflictException(topic, topic_type, msg_type_string)
# Create the subscriber and associated member variables
# Subscriptions is initialized with the current client to start with.
self.subscriptions = {client_id: callback}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.node_handle = node_handle
# TODO(@jubeira): add support for other QoS.
self.subscriber = node_handle.create_subscription(
msg_class, topic, self.callback, 10)
self.new_subscriber = None
self.new_subscriptions = {}
def test_publish_twice(self):
""" Make sure that publishing works """
topic = "/test_publish_twice"
msg_type = "std_msgs/String"
msg = {"data": "why halo thar"}
received = {"msg": None}
def cb(msg):
received["msg"] = msg
rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
p = MultiPublisher(topic, msg_type)
p.publish(msg)
sleep(1)
self.assertEqual(received["msg"].data, msg["data"])
p.unregister()
sleep(5)
received["msg"] = None
self.assertIsNone(received["msg"])
p = MultiPublisher(topic, msg_type)
p.publish(msg)
sleep(1)
self.assertEqual(received["msg"].data, msg["data"])
def on_event(self, message):
#process the message
#replace JSON Null values in float32 types with infinity datatype (changed according to the error for LaserScan values)
message = message.replace("null", "Infinity")
msg_dict = json.loads(message)
topic_name = msg_dict['topic_name']
topic_type = msg_dict['topic_type']
del msg_dict['topic_name']
del msg_dict['topic_type']
#get the topic type
msg_class = ros_loader.get_message_class(topic_type)
# Create a message instance
inst = msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg_dict, inst)
if not topic_name in self.pub_objects:
self.pub_objects[topic_name] = rospy.Publisher(topic_name,
msg_class,
latch=True,
queue_size=10)
self.pub_objects[topic_name].publish(inst)
def test_publish_twice(self):
""" Make sure that publishing works """
topic = "/test_publish_twice"
msg_type = "std_msgs/String"
msg = {"data": "why halo thar"}
received = {"msg": None}
def cb(msg):
received["msg"] = msg
rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
p = MultiPublisher(topic, msg_type)
p.publish(msg)
sleep(1)
self.assertEqual(received["msg"].data, msg["data"])
p.unregister()
sleep(5)
received["msg"] = None
self.assertIsNone(received["msg"])
p = MultiPublisher(topic, msg_type)
p.publish(msg)
sleep(1)
self.assertEqual(received["msg"].data, msg["data"])
def test_irregular_msgnames(self):
irregular = ["std_msgs//String", "//std_msgs/String",
"/std_msgs//String", "/std_msgs/String", "//std_msgs//String",
"/std_msgs/String/", "//std_msgs//String//", "std_msgs/String/",
"std_msgs//String//"]
for x in irregular:
self.assertNotEqual(ros_loader.get_message_class(x), None)
self.assertNotEqual(ros_loader.get_message_instance(x), None)
def test_irregular_msgnames(self):
irregular = ["std_msgs//String", "//std_msgs/String",
"/std_msgs//String", "/std_msgs/String", "//std_msgs//String",
"/std_msgs/String/", "//std_msgs//String//", "std_msgs/String/",
"std_msgs//String//"]
for x in irregular:
self.assertNotEqual(ros_loader.get_message_class(x), None)
self.assertNotEqual(ros_loader.get_message_instance(x), None)
def handle_ros_subscribers(self):
Subscriber_Handler.session = self
for topic_name in pub2net_topics:
topic_type = get_topic_type(topic_name)[0]
msg_class = ros_loader.get_message_class(topic_type)
subscriber_Handler = Subscriber_Handler(topic_name,topic_type)
rospy.Subscriber(topic_name, msg_class, subscriber_Handler.callback)
def test_assorted_msgnames(self):
assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
"geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
"nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String",
"trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
"visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
"sensor_msgs/PointCloud2"]
for x in assortedmsgs:
self.assertNotEqual(ros_loader.get_message_class(x), None)
inst = ros_loader.get_message_instance(x)
self.assertNotEqual(inst, None)
self.assertEqual(x, inst._type)
def test_assorted_msgnames(self):
assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
"geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
"nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String",
"trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
"visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
"sensor_msgs/PointCloud2"]
for x in assortedmsgs:
self.assertNotEqual(ros_loader.get_message_class(x), None)
inst = ros_loader.get_message_instance(x)
self.assertNotEqual(inst, None)
self.assertEqual(x, inst._type)
def __init__(self,
topic,
msg_type=None,
latched_client_id=None,
queue_size=100):
""" Register a publisher on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the publisher to
msg_type -- (optional) the type to register the publisher as. If not
provided, an attempt will be made to infer the topic type
latch -- (optional) if a client requested this publisher to be latched,
provide the client_id of that client here
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the publisher and associated member variables
self.clients = {}
self.latched_client_id = latched_client_id
self.topic = topic
self.msg_class = msg_class
self.publisher = Publisher(topic,
msg_class,
latch=(latched_client_id != None),
queue_size=queue_size)
self.listener = PublisherConsistencyListener()
self.listener.attach(self.publisher)
def verify_type(self, msg_type):
""" Verify that the publisher publishes messages of the specified type.
Keyword arguments:
msg_type -- the type to check this publisher against
Throws:
Exception -- if ros_loader cannot load the specified msg type
TypeConflictException -- if the msg_type is different than the type of
this publisher
"""
if not ros_loader.get_message_class(msg_type) is self.msg_class:
raise TypeConflictException(self.topic,
self.msg_class._type, msg_type)
return
def verify_type(self, msg_type):
""" Verify that the subscriber subscribes to messages of this type.
Keyword arguments:
msg_type -- the type to check this subscriber against
Throws:
Exception -- if ros_loader cannot load the specified msg type
TypeConflictException -- if the msg_type is different than the type of
this publisher
"""
if not ros_loader.get_message_class(msg_type) is self.msg_class:
raise TypeConflictException(self.topic, self.msg_class._type,
msg_type)
return
def test_std_msgnames(self):
stdmsgs = ["std_msgs/Bool", "std_msgs/Byte", "std_msgs/ByteMultiArray",
"std_msgs/ColorRGBA", "std_msgs/Duration", "std_msgs/Empty",
"std_msgs/Float32", "std_msgs/Float32MultiArray", "std_msgs/Float64",
"std_msgs/Header", "std_msgs/Int16", "std_msgs/Int16MultiArray",
"std_msgs/Int32", "std_msgs/Int32MultiArray", "std_msgs/Int64",
"std_msgs/Int64MultiArray", "std_msgs/Int8", "std_msgs/Int8MultiArray",
"std_msgs/MultiArrayDimension", "std_msgs/MultiArrayLayout",
"std_msgs/String", "std_msgs/Time", "std_msgs/UInt16",
"std_msgs/UInt16MultiArray", "std_msgs/UInt32MultiArray",
"std_msgs/UInt64MultiArray", "std_msgs/UInt32", "std_msgs/UInt64",
"std_msgs/UInt8", "std_msgs/UInt8MultiArray"]
for x in stdmsgs:
self.assertNotEqual(ros_loader.get_message_class(x), None)
inst = ros_loader.get_message_instance(x)
self.assertNotEqual(inst, None)
self.assertEqual(x, inst._type)
def test_std_msgnames(self):
stdmsgs = ["std_msgs/Bool", "std_msgs/Byte", "std_msgs/ByteMultiArray",
"std_msgs/ColorRGBA", "std_msgs/Duration", "std_msgs/Empty",
"std_msgs/Float32", "std_msgs/Float32MultiArray", "std_msgs/Float64",
"std_msgs/Header", "std_msgs/Int16", "std_msgs/Int16MultiArray",
"std_msgs/Int32", "std_msgs/Int32MultiArray", "std_msgs/Int64",
"std_msgs/Int64MultiArray", "std_msgs/Int8", "std_msgs/Int8MultiArray",
"std_msgs/MultiArrayDimension", "std_msgs/MultiArrayLayout",
"std_msgs/String", "std_msgs/Time", "std_msgs/UInt16",
"std_msgs/UInt16MultiArray", "std_msgs/UInt32MultiArray",
"std_msgs/UInt64MultiArray", "std_msgs/UInt32", "std_msgs/UInt64",
"std_msgs/UInt8", "std_msgs/UInt8MultiArray"]
for x in stdmsgs:
self.assertNotEqual(ros_loader.get_message_class(x), None)
inst = ros_loader.get_message_instance(x)
self.assertNotEqual(inst, None)
self.assertEqual(x, inst._type)
def test_publish_twice(self):
""" Make sure that publishing works """
topic = "/test_publish_twice"
msg_type = "std_msgs/String"
msg = {"data": "why halo thar"}
received = {"msg": None}
def cb(msg):
received["msg"] = msg
rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
p = MultiPublisher(topic, msg_type)
p.publish(msg)
sleep(1)
self.assertEqual(received["msg"].data, msg["data"])
p.unregister()
# The publisher went away at time T. Here's the timeline of the events:
# T+1 seconds - the subscriber will retry to reconnect - fail
# T+3 seconds - the subscriber will retry to reconnect - fail
# T+5 seconds - publish msg -> it's gone
# T+7 seconds - the subscriber will retry to reconnect - success
# T+8 seconds - publish msg -> OK
# T+11 seconds - we receive the message. Looks like a bug in reconnection...
# https://github.com/ros/ros_comm/blob/indigo-devel/clients/rospy/src/rospy/impl/tcpros_base.py#L733
# That line should probably be indented.
sleep(5)
received["msg"] = None
self.assertIsNone(received["msg"])
p = MultiPublisher(topic, msg_type)
p.publish(msg)
self.assertIsNone(received["msg"])
sleep(3)
p.publish(msg)
sleep(2)
# Next two lines should be removed when this is fixed:
# https://github.com/ros/ros_comm/blob/indigo-devel/clients/rospy/src/rospy/impl/tcpros_base.py#L733
self.assertIsNone(received["msg"])
sleep(3)
self.assertEqual(received["msg"].data, msg["data"])
def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100):
""" Register a publisher on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the publisher to
msg_type -- (optional) the type to register the publisher as. If not
provided, an attempt will be made to infer the topic type
latch -- (optional) if a client requested this publisher to be latched,
provide the client_id of that client here
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the publisher and associated member variables
self.clients = {}
self.latched_client_id = latched_client_id
self.topic = topic
self.msg_class = msg_class
self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size)
self.listener = PublisherConsistencyListener()
self.listener.attach(self.publisher)
def __init__(self, topic, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
if msg_type == "__AnyMsg":
msg_class = AnyMsg
else:
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the subscriber and associated member variables
self.subscriptions = {}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.subscriber = Subscriber(topic, msg_class, self.callback)
def __init__(self, topic, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the subscriber and associated member variables
self.subscriptions = {}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.subscriber = Subscriber(topic, msg_class, self.callback)
def __init__(self,
topic,
node_handle,
msg_type=None,
latched_client_id=None,
queue_size=100):
""" Register a publisher on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the publisher to
node_handle -- Handle to a rclpy node to create the publisher.
msg_type -- (optional) the type to register the publisher as. If not
provided, an attempt will be made to infer the topic type
latch -- (optional) if a client requested this publisher to be latched,
provide the client_id of that client here
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topics_names_and_types = dict(node_handle.get_topic_names_and_types())
topic_type = topics_names_and_types.get(topic)
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# topic_type is a list of types or None at this point; only one type is supported.
if topic_type is not None:
if len(topic_type) > 1:
node_handle.get_logger().warning(
'More than one topic type detected: {}'.format(topic_type))
topic_type = topic_type[0]
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
msg_type_string = msg_class_type_repr(msg_class)
if topic_type is not None and topic_type != msg_type_string:
raise TypeConflictException(topic, topic_type, msg_type_string)
# Create the publisher and associated member variables
self.clients = {}
self.latched_client_id = latched_client_id
self.topic = topic
self.node_handle = node_handle
self.msg_class = msg_class
# Adding a lifespan solves the problem of late-joining subscribers
# without the need of a custom message publisher implementation.
publisher_qos = QoSProfile(depth=queue_size,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
# For latched clients, no lifespan has to be specified (i.e. latch forever).
# Otherwise we want to keep the messages for a second to prevent late-joining subscribers from
# missing messages.
if latched_client_id is None:
publisher_qos.lifespan = Duration(seconds=1)
else:
publisher_qos.depth = 1
self.publisher = node_handle.create_publisher(
msg_class, topic, qos_profile=publisher_qos)
