def __init__(self):
super().__init__('simple_trajectory')
# Parameters
self.length_m = self.declare_parameter("length_m", 20.0).value
self.min_speed_mps = self.declare_parameter("min_speed_mps", 1.0).value
self.max_speed_mps = self.declare_parameter("max_speed_mps", 8.0).value
self.discretization_m = self.declare_parameter("discretization_m", 0.5).value
self.speed_increments = self.declare_parameter("speed_increments", 0.2).value
self.offset_distnace_m = self.declare_parameter("offset_distnace_m", 0.0).value
self.one_time = self.declare_parameter("one_time", False).value
self.timer_period_sec = self.declare_parameter("timer_period_sec", 0.0).value
if self.max_speed_mps < self.min_speed_mps:
self.max_speed_mps = self.min_speed_mps
# Publisher / Subscriber
qos = QoSProfile(depth=10)
if (self.one_time):
qos.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
qos.depth = 1
self.publisher_trajectory_ = self.create_publisher(
Trajectory, "trajectory", qos
)
self.subscriber_vehicle_kinematic_state_ = self.create_subscription(
VehicleKinematicState, "vehicle_kinematic_state",
self.vehicle_kinematic_state_cb, 0
)
# Local variables
self.trajectory_msg_ = None
self.vehicle_kinematic_state_msg_ = None
if (not self.one_time):
self.timer = self.create_timer(self.timer_period_sec, self.publish_trajectory)
def run_topic_listening(node, topic_monitor, options):
"""Subscribe to relevant topics and manage the data received from susbcriptions."""
already_ignored_topics = set()
while rclpy.ok():
# Check if there is a new topic online
# TODO(dhood): use graph events rather than polling
topic_names_and_types = node.get_topic_names_and_types()
for topic_name, type_names in topic_names_and_types:
# Infer the appropriate QoS profile from the topic name
topic_info = topic_monitor.get_topic_info(topic_name)
if topic_info is None:
# The topic is not for being monitored
continue
if len(type_names) != 1:
if topic_name not in already_ignored_topics:
node.get_logger().info(
"Warning: ignoring topic '%s', which has more than one type: [%s]"
% (topic_name, ', '.join(type_names)))
already_ignored_topics.add(topic_name)
continue
type_name = type_names[0]
if not topic_monitor.is_supported_type(type_name):
if topic_name not in already_ignored_topics:
node.get_logger().info(
"Warning: ignoring topic '%s' because its message type (%s)"
'is not suported.' % (topic_name, type_name))
already_ignored_topics.add(topic_name)
continue
is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics
if is_new_topic:
# Register new topic with the monitor
qos_profile = QoSProfile(depth=10)
qos_profile.depth = QOS_DEPTH
if topic_info['reliability'] == 'best_effort':
qos_profile.reliability = \
QoSReliabilityPolicy.BEST_EFFORT
topic_monitor.add_monitored_topic(Header, topic_name, node,
qos_profile,
options.expected_period,
options.allowed_latency,
options.stale_time)
# Wait for messages with a timeout, otherwise this thread will block any other threads
# until a message is received
rclpy.spin_once(node, timeout_sec=0.05)
def __init__(self):
super().__init__('minimal_publisher')
qos_at_start = QoSProfile()
print("Configuring", end=',')
qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
qos_at_start.depth = 10
qos_at_start.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
qos_at_start.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
print("Configured")
#self.get_logger().info('Publishing: "%s"' % qos_profile_at_start)
#print(" ", qos_at_start)
self.publisher_ = self.create_publisher(String,
'topic',
qos_profile=qos_at_start)
print(self.publisher_)
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
def __init__(self):
super().__init__('minimal_subscriber')
qos_at_start = QoSProfile()
qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
#qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
qos_at_start.depth = 10
qos_at_start.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
qos_at_start.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
self.group = ThrottledCallbackGroup(self)
self.subscription = self.create_subscription(String,
'topic',
self.listener_callback,
qos_profile=qos_at_start,
callback_group=self.group)
print(self.subscription)
self.subscription # prevent unused variable warning
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)
def main():
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'data_name', nargs='?', default='topic1',
help='Name of the data (must comply with ROS topic rules)')
parser.add_argument(
'--best-effort', action='store_true', default=False,
help='Set QoS reliability option to "best effort"')
parser.add_argument(
'--transient-local', action='store_true', default=False,
help='Set QoS durability option to "transient local"')
parser.add_argument(
'--depth', type=int, default=default_depth, help='Size of the QoS history depth')
parser.add_argument(
'--keep-all', action='store_true', default=False,
help='Set QoS history option to "keep all" (unlimited depth, subject to resource limits)')
parser.add_argument(
'--payload-size', type=int, default=0, help='Size of data payload to send')
parser.add_argument(
'--period', type=float, default=0.5, help='Time in seconds between messages')
parser.add_argument(
'--end-after', type=int, help='Script will exit after publishing this many messages')
args = parser.parse_args()
reliability_suffix = '_best_effort' if args.best_effort else ''
topic_name = '{0}_data{1}'.format(args.data_name, reliability_suffix)
rclpy.init()
node = rclpy.create_node('%s_pub' % topic_name)
node_logger = node.get_logger()
qos_profile = QoSProfile()
if args.best_effort:
node_logger.info('Reliability: best effort')
qos_profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
else:
node_logger.info('Reliability: reliable')
qos_profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
if args.keep_all:
node_logger.info('History: keep all')
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
else:
node_logger.info('History: keep last')
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
node_logger.info('Depth: {0}'.format(args.depth))
qos_profile.depth = args.depth
if args.transient_local:
node_logger.info('Durability: transient local')
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
else:
node_logger.info('Durability: volatile')
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
data_pub = node.create_publisher(
Header, topic_name, qos_profile=qos_profile)
node_logger.info('Publishing on topic: {0}'.format(topic_name))
node_logger.info('Payload size: {0}'.format(args.payload_size))
data = 'a' * args.payload_size
msg = Header()
cycle_count = 0
def publish_msg(val):
msg.frame_id = '{0}_{1}'.format(val, data)
data_pub.publish(msg)
node_logger.info('Publishing: "{0}"'.format(val))
while rclpy.ok():
if args.end_after is not None and cycle_count >= args.end_after:
publish_msg(-1)
sleep(0.1) # allow reliable messages to get re-sent if needed
return
publish_msg(cycle_count)
cycle_count += 1
try:
sleep(args.period)
except KeyboardInterrupt:
publish_msg(-1)
sleep(0.1)
raise
