def _qos_profile(history, depth, reliability, durability):
# depth
profile = QoSProfile(depth=depth)
# history
if history.lower() == "keep_all":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
elif history.lower() == "keep_last":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
else:
raise RuntimeError("Invalid history policy: %s" % history)
# reliability
if reliability.lower() == "reliable":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
elif reliability.lower() == "best_effort":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
else:
raise RuntimeError("Invalid reliability policy: %s" % reliability)
# durability
if durability.lower() == "transient_local":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
elif durability.lower() == "volatile":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
else:
raise RuntimeError("Invalid durability policy: %s" % durability)
return profile
def __init__(self):
super().__init__("simple_publisher")
logger = self.get_logger()
self._num = 0
period = self.declare_parameter("period", 1.0)
depth = self.declare_parameter("qos_depth", 5)
logger.info("param period={}"
.format(period.get_parameter_value().double_value))
logger.info("param qos_depth={}"
.format(depth.get_parameter_value().integer_value))
profile = QoSProfile(depth=depth.get_parameter_value().integer_value)
profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
profile.liveliness = QoSLivelinessPolicy.AUTOMATIC
self._pub = self.create_publisher(SimpleMsg, "sample_topic", profile)
self.create_timer(
period.get_parameter_value().double_value,
self.on_timer_callback)
logger.info("publisher start!")
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 __init__(self):
self.node = ros2.create_node("imu_sub")
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
qos_profile.reliability = (
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
# subscribe to the accel topic, which will call
# __accel_callback every time the camera publishes data
self.__accel_sub = self.node.create_subscription(
Imu, self.__ACCEL_TOPIC, self.__accel_callback, qos_profile
)
# subscribe to the gyro topic, which will call
# __gyro_callback every time the camera publishes data
self.__gyro_sub = self.node.create_subscription(
Imu, self.__GYRO_TOPIC, self.__gyro_callback, qos_profile
)
self.__acceleration = np.array([0, 0, 0])
self.__acceleration_buffer = deque()
self.__angular_velocity = np.array([0, 0, 0])
self.__angular_velocity_buffer = deque()
def retrieve_urdf(self, timeout_sec: float = 5) -> None:
"""Retrieve the URDF file from the /robot_description topic.
Will raise an EnvironmentError if the topic is unavailable.
"""
self.logger.info('Retrieving URDF from "/robot_description"...')
qos_profile = QoSProfile(depth=1)
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
self.urdf = None
def urdf_received(msg: String):
self.urdf = msg.data
self.create_subscription(
msg_type=String,
topic='/robot_description',
qos_profile=qos_profile,
callback=urdf_received,
)
rclpy.spin_once(self, timeout_sec=timeout_sec)
if self.urdf is None:
self.logger.error('Could not retrieve the URDF!')
raise EnvironmentError('Could not retrieve the URDF!')
def __init__(self):
self.__bridge = CvBridge()
# ROS node
self.node = ros2.create_node("image_sub")
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
qos_profile.reliability = (
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
# subscribe to the color image topic, which will call
# __color_callback every time the camera publishes data
self.__color_image_sub = self.node.create_subscription(
Image, self.__COLOR_TOPIC, self.__color_callback, qos_profile
)
self.__color_image = None
self.__color_image_new = None
# subscribe to the depth image topic, which will call
# __depth_callback every time the camera publishes data
self.__depth_image_sub = self.node.create_subscription(
Image, self.__DEPTH_TOPIC, self.__depth_callback, qos_profile
)
self.__depth_image = None
self.__depth_image_new = None
def _getQoSProfile(self):
profile = QoSProfile(depth=10)
profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
profile.liveliness = QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
profile.deadline = Duration()
profile.lifespan = Duration()
return profile
def __init__(self):
qos = QoSProfile(depth=10)
qos.durability = DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(String, 'topic',
self.listener_callback,
qos)
def _getQoSProfile(self):
profile = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL)
profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
profile.liveliness = QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
profile.deadline = Duration()
profile.lifespan = Duration()
return profile
def __init__(self, robot_description):
super().__init__('robot_desc_publisher')
qos_profile = QoSProfile(depth=1)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
self.publisher_ = self.create_publisher(String, 'robot_description',
qos_profile)
self.robot_description = robot_description
self.timer_callback()
def qos_profile(d):
history = d["history"] # keep_last|keep_all
depth = d["depth"] # used if using keep_last
reliability = d["reliability"] # reliable|best_effort
durability = d["durability"] # transient_local|volatile
# depth
profile = QoSProfile(depth = depth)
# history
if history == "keep_all":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
elif history == "keep_last":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
else:
raise RuntimeError("Invalid history policy: %s"%history)
# reliability
if reliability == "reliable":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
elif reliability == "best_effort":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
else:
raise RuntimeError("Invalid reliability policy: %s"%reliability)
# durability
if durability == "transient_local":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
elif durability == "volatile":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
else:
raise RuntimeError("Invalid durability policy: %s"%durability)
return profile
def __init__(self):
qos = QoSProfile(depth=10)
qos.durability = DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
custom_callback = lambda event: print("Custom Incompatible Callback!")
callbacks = SubscriptionEventCallbacks()
callbacks.incompatible_qos = custom_callback
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(
String,
'topic',
self.listener_callback,
qos,
event_callbacks=callbacks)
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):
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__("simple_subscription")
logger = self.get_logger()
self._num = 0
depth = self.declare_parameter("qos_depth", 5)
logger.info("param qos_depth={}".format(
depth.get_parameter_value().integer_value))
profile = QoSProfile(depth=depth.get_parameter_value().integer_value)
profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
profile.liveliness = QoSLivelinessPolicy.AUTOMATIC
self.create_subscription(SimpleMsg, "sample_topic",
self.on_message_callback, profile)
logger.info("subscription start!")
#!/usr/bin/python3
import time
import rclpy
from gateway_interfaces.msg import SomeRequest, SomeResponse
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
_NODE_NAME = 'some_request_to_some_response'
_REQUEST_TOPIC = 'some_request'
_RESPONSE_TOPIC = 'some_response'
_QOS_RESPONSE = QoSProfile(depth=1)
_QOS_RESPONSE.durability = QoSDurabilityPolicy.SYSTEM_DEFAULT
_QOS_RESPONSE.history = QoSHistoryPolicy.SYSTEM_DEFAULT
_QOS_RESPONSE.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT
_QOS_REQUEST = QoSProfile(depth=1)
_QOS_REQUEST.durability = QoSDurabilityPolicy.SYSTEM_DEFAULT
_QOS_REQUEST.history = QoSHistoryPolicy.SYSTEM_DEFAULT
_QOS_REQUEST.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT
_SOURCE_ID = 1
class SomeRequestToSomeResponseNode(Node):
def __init__(self):
super().__init__(_NODE_NAME)
self.logger = self.get_logger()
def __init__(self):
print("Env initialized")
#define spaces
self.max_v = 9.5
self.min_v = -9.5
self.max_obs = 2.0
self.min_obs = -2.0
self.iter = 0.0
self.time = 0
self.time_0 = 0
#Parameters Reward function
self.a = 1000
self.b = 35
self.action_space = spaces.Box(self.min_v,
self.max_v,
shape=(4, ),
dtype=np.float32)
self.observation_space = spaces.Box(self.min_obs,
self.max_obs,
shape=(12, ),
dtype=np.float32)
self.reward_range = (-np.inf, 0)
#create ROS2 node
rclpy.init(args=None)
self.node = rclpy.create_node("pr_env")
#initialize variables
self.target_position = np.array(
[0.830578, 0.847096, 0.831132, 0.792133])
first_position = np.array([0.679005, 0.708169, 0.684298, 0.637145])
first_velocity = np.array([0.0, 0.0, 0.0, 0.0])
self.first_obs = np.concatenate(
(first_position, first_velocity,
(self.target_position - first_position)),
axis=0)
self.obs = self.first_obs
self.status = "starting"
self.time_obs_ns = 0
self.time_obs_ns_ = self.time_obs_ns
#create ROS2 communication
self.publisher_ = self.node.create_publisher(PRArrayH,
'control_action', 1)
self.subscription_ = self.node.create_subscription(
PRState, 'pr_state', self._state_callback, 1)
self.publisher_reset_vel = self.node.create_publisher(
Bool, 'der_reset', 1)
sim_qos = QoSProfile(depth=1)
sim_qos.reliability = QoSReliabilityPolicy.BEST_EFFORT
sim_qos.history = QoSHistoryPolicy.KEEP_LAST
sim_qos.durability = QoSDurabilityPolicy.VOLATILE
self.subscription_sim_ = self.node.create_subscription(
Quaternion, "posicion_sim", self._sim_callback, sim_qos)
#create pub and sub for matlab macro
self.publisher_start_ = self.node.create_publisher(
Bool, "start_flag", 1)
self.subscription_end_ = self.node.create_subscription(
String, "status_sim", self._end_callback, 1)
self.seed()
start = input('Press any key to start')
def main(args=None):
# Argument parsing and usage
parser = get_parser()
parsed_args = parser.parse_args()
# Configuration variables
qos_policy_name = parsed_args.incompatible_qos_policy_name
qos_profile_publisher = QoSProfile(depth=10)
qos_profile_subscription = QoSProfile(depth=10)
if qos_policy_name == 'durability':
print('Durability incompatibility selected.\n'
'Incompatibility condition: publisher durability kind <'
'subscripition durability kind.\n'
'Setting publisher durability to: VOLATILE\n'
'Setting subscription durability to: TRANSIENT_LOCAL\n')
qos_profile_publisher.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
qos_profile_subscription.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
elif qos_policy_name == 'deadline':
print(
'Deadline incompatibility selected.\n'
'Incompatibility condition: publisher deadline > subscription deadline.\n'
'Setting publisher durability to: 2 seconds\n'
'Setting subscription durability to: 1 second\n')
qos_profile_publisher.deadline = Duration(seconds=2)
qos_profile_subscription.deadline = Duration(seconds=1)
elif qos_policy_name == 'liveliness_policy':
print('Liveliness Policy incompatibility selected.\n'
'Incompatibility condition: publisher liveliness policy <'
'subscripition liveliness policy.\n'
'Setting publisher liveliness policy to: AUTOMATIC\n'
'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n')
qos_profile_publisher.liveliness = \
QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
qos_profile_subscription.liveliness = \
QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC
elif qos_policy_name == 'liveliness_lease_duration':
print(
'Liveliness lease duration incompatibility selected.\n'
'Incompatibility condition: publisher liveliness lease duration >'
'subscription liveliness lease duration.\n'
'Setting publisher liveliness lease duration to: 2 seconds\n'
'Setting subscription liveliness lease duration to: 1 second\n')
qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2)
qos_profile_subscription.liveliness_lease_duration = Duration(
seconds=1)
elif qos_policy_name == 'reliability':
print(
'Reliability incompatibility selected.\n'
'Incompatibility condition: publisher reliability < subscripition reliability.\n'
'Setting publisher reliability to: BEST_EFFORT\n'
'Setting subscription reliability to: RELIABLE\n')
qos_profile_publisher.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
qos_profile_subscription.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
else:
print('{name} not recognised.'.format(name=qos_policy_name))
parser.print_help()
return 0
# Initialization and configuration
rclpy.init(args=args)
topic = 'incompatible_qos_chatter'
num_msgs = 5
publisher_callbacks = PublisherEventCallbacks(
incompatible_qos=lambda event: get_logger('Talker').info(str(event)))
subscription_callbacks = SubscriptionEventCallbacks(
incompatible_qos=lambda event: get_logger('Listener').info(str(event)))
try:
talker = Talker(topic,
qos_profile_publisher,
event_callbacks=publisher_callbacks,
publish_count=num_msgs)
listener = Listener(topic,
qos_profile_subscription,
event_callbacks=subscription_callbacks)
except UnsupportedEventTypeError as exc:
print()
print(exc, end='\n\n')
print('Please try this demo using a different RMW implementation')
return -1
executor = SingleThreadedExecutor()
executor.add_node(listener)
executor.add_node(talker)
try:
while talker.publish_count < num_msgs:
executor.spin_once()
except KeyboardInterrupt:
pass
rclpy.shutdown()
return 0
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(depth=args.depth)
if args.best_effort:
node_logger.info('Reliability: best effort')
qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
else:
node_logger.info('Reliability: reliable')
qos_profile.reliability = QoSReliabilityPolicy.RELIABLE
if args.keep_all:
node_logger.info('History: keep all')
qos_profile.history = QoSHistoryPolicy.KEEP_ALL
else:
node_logger.info('History: keep last')
qos_profile.history = QoSHistoryPolicy.KEEP_LAST
node_logger.info('Depth: {0}'.format(args.depth))
if args.transient_local:
node_logger.info('Durability: transient local')
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
else:
node_logger.info('Durability: volatile')
qos_profile.durability = QoSDurabilityPolicy.VOLATILE
data_pub = node.create_publisher(Header, topic_name, 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
