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