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__('jarvis_camera') self.declare_parameters( namespace='', parameters=[ ('width', 640), ('height', 480), ('camera_topic', "camera_image"), ('time_period', 0.2), ] ) timer_period = self.get_parameter("time_period").value self.width = self.get_parameter("width").value self.height = self.get_parameter("height").value qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT self.pub = self.create_publisher(CompressedImage, self.get_parameter("camera_topic").value, qos_profile) self.timer = self.create_timer(timer_period, self.timer_callback) self.frame = None self.has_frame = False # self.bridge = CvBridge() #cv2.VideoCapture( # "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)1280, height=(int)720,format=(string)NV12, framerate=(fraction)24/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink") self.get_logger().info("topic: %s, w: %d, h: %d, period: %.3f"%(self.get_parameter("camera_topic").value, self.width, self.height, timer_period))
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 __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): 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 _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 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 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__('translator') self.posearray = [] qos_profile = QoSProfile(depth=1) qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT self.sub_drone_tf = self.create_subscription(TransformStamped, "/drone/tf", self.drone_tf_callback, qos_profile) self.sub_drone_pose = self.create_subscription( PoseStamped, "/drone/pose", self.drone_pose_callback, qos_profile) self.pub_tf = self.create_publisher(TFMessage, "/tf", QoSProfile(depth=1)) self.pub_posearray = self.create_publisher(Path, "/drone/path", QoSProfile(depth=1))
def __init__(self): super().__init__('camera_viewer') self.declare_parameters(namespace='', parameters=[('time_period', 0.033), ('camera_topic', '/jarvis/camera') ]) self.timer = self.create_timer( self.get_parameter("time_period").value, self.timer_callback) qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT self.sub = self.create_subscription( CompressedImage, self.get_parameter("camera_topic").value, self.callback, qos_profile) self.frame = None self.has_frame = False
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): 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!")
def __init__(self): """Create a SoftwareUpdateNode. """ super().__init__("software_update_process") self.get_logger().info("software_update_process started") # Scheduler to queue the function calls and run them in a separate thread. self.scheduler = scheduler.Scheduler(self.get_logger()) # Threading lock object to safely update the update_state variable. self.state_guard = threading.Lock() self.update_state = software_update_config.SoftwareUpdateState.UPDATE_UNKNOWN # Threading Event object to indicate if the software update check is completed. self.check_complete = threading.Event() # Flag to indicate software update check in progress. self.check_in_progress = False # List of packages that have updates available. self.update_list = list() # Flag to identify if the network is connected. self.is_network_connected = False # Flag to identify if software update check has to performed # again when network is connected. self.reschedule_software_update_check = False # The apt cache object. self.cache = apt.Cache() # Double buffer object containing the current update percentage and progress state. self.pct_dict_db = utility.DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({ software_update_config.PROG_STATE_KEY: software_update_config.PROGRESS_STATES[0], software_update_config.PROG_PCT_KEY: 0.0 }) # Timer to periodically check for software update. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.get_logger().info( "Schedule the software update check every " f"{software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS} " "seconds.") self.schedule_update_check_cb = ReentrantCallbackGroup() self.update_check_timer = \ self.create_timer(software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS, self.schedule_update_check, callback_group=self.schedule_update_check_cb) # Publisher that sends software update pct and status. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.pct_obj = SoftwareUpdatePctMsg() self.software_update_pct_pub_cb = ReentrantCallbackGroup() self.software_update_pct_publisher = \ self.create_publisher(SoftwareUpdatePctMsg, software_update_config.SOFTWARE_UPDATE_PCT_TOPIC_NAME, qos_profile=qos_profile, callback_group=self.software_update_pct_pub_cb) # Service to check if there is a software update available. self.software_update_check_cb_group = ReentrantCallbackGroup() self.software_update_check_service = \ self.create_service(SoftwareUpdateCheckSrv, software_update_config.SOFTWARE_UPDATE_CHECK_SERVICE_NAME, self.software_update_check, callback_group=self.software_update_check_cb_group) # Service to execute the software update and install latest packages. self.begin_update_service_cb_group = ReentrantCallbackGroup() self.begin_update_service = \ self.create_service(BeginSoftwareUpdateSrv, software_update_config.BEGIN_SOFTWARE_UPDATE_SERVICE_NAME, self.begin_update, callback_group=self.begin_update_service_cb_group) # Service to get the current software update state. self.software_update_state_service_cb_group = ReentrantCallbackGroup() self.software_update_state_service = \ self.create_service(SoftwareUpdateStateSrv, software_update_config.SOFTWARE_UPDATE_STATE_SERVICE_NAME, self.software_update_state, callback_group=self.software_update_state_service_cb_group) self.nw_con_status_cb_group = ReentrantCallbackGroup() self.network_connection_status_sub = \ self.create_subscription(NetworkConnectionStatus, software_update_config.NETWORK_CONNECTION_STATUS_TOPIC_NAME, self.network_connection_status_cb, 10, callback_group=self.nw_con_status_cb_group) # Clients to Status LED services that are called to indicate progress/success/failure # status while loading model. self.led_cb_group = MutuallyExclusiveCallbackGroup() self.led_blink_service = self.create_client( SetStatusLedBlinkSrv, constants.LED_BLINK_SERVICE_NAME, callback_group=self.led_cb_group) self.led_solid_service = self.create_client( SetStatusLedSolidSrv, constants.LED_SOLID_SERVICE_NAME, callback_group=self.led_cb_group) while not self.led_blink_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led blink service not available, waiting again...") while not self.led_solid_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led solid service not available, waiting again...") self.led_blink_request = SetStatusLedBlinkSrv.Request() self.led_solid_request = SetStatusLedSolidSrv.Request() # Client to USB File system subscription service that allows the node to install # signed software packages(*.deb.gpg) present in the USB. The usb_monitor_node # will trigger notification if it finds the "update" folder from the watchlist # in the USB drive. self.usb_sub_cb_group = ReentrantCallbackGroup() self.usb_file_system_subscribe_client = self.create_client( USBFileSystemSubscribeSrv, constants.USB_FILE_SYSTEM_SUBSCRIBE_SERVICE_NAME, callback_group=self.usb_sub_cb_group) while not self.usb_file_system_subscribe_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "File System Subscribe not available, waiting again...") # Client to USB Mount point manager service to indicate that the usb_monitor_node can safely # decrement the counter for the mount point once the action function for the "update" folder # file being watched by software_update_node is succesfully executed. self.usb_mpm_cb_group = ReentrantCallbackGroup() self.usb_mount_point_manager_client = self.create_client( USBMountPointManagerSrv, constants.USB_MOUNT_POINT_MANAGER_SERVICE_NAME, callback_group=self.usb_mpm_cb_group) while not self.usb_mount_point_manager_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "USB mount point manager service not available, waiting again..." ) # Subscriber to USB File system notification publisher to recieve the broadcasted messages # with file/folder details, whenever a watched file is identified in the USB connected. self.usb_notif_cb_group = ReentrantCallbackGroup() self.usb_file_system_notification_sub = self.create_subscription( USBFileSystemNotificationMsg, constants.USB_FILE_SYSTEM_NOTIFICATION_TOPIC, self.usb_file_system_notification_cb, 10, callback_group=self.usb_notif_cb_group) # Add the "update" folder to the watchlist. usb_file_system_subscribe_request = USBFileSystemSubscribeSrv.Request() usb_file_system_subscribe_request.file_name = software_update_config.UPDATE_SOURCE_DIRECTORY usb_file_system_subscribe_request.callback_name = software_update_config.SCHEDULE_USB_UPDATE_SCAN_CB usb_file_system_subscribe_request.verify_name_exists = True self.usb_file_system_subscribe_client.call_async( usb_file_system_subscribe_request) # Set the power led to blue. self.led_solid_request.led_index = constants.LEDIndex.POWER_LED self.led_solid_request.color = constants.LEDColor.BLUE self.led_solid_request.hold = 0.0 self.led_solid_service.call_async(self.led_solid_request) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback) # Schedule update check. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.schedule_update_check() self.get_logger().info("Software Update node successfully created")
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
def __init__(self): """Create a WebServerNode and launch a flask server with default host and port details. """ super().__init__("webserver_publisher_node") self.get_logger().info("webserver_publisher_node started") HOST_DEFAULT = "0.0.0.0" PORT_DEFAULT = "5001" self.get_logger().info(f"Running the flask server on {HOST_DEFAULT}:{PORT_DEFAULT}") # Run the Flask webserver as a background thread. self.get_logger().info("Running webserver") self.server_thread = threading.Thread(target=app.run, daemon=True, kwargs={ "host": HOST_DEFAULT, "port": PORT_DEFAULT, "use_reloader": False, "threaded": True} ) self.server_thread.start() # Create service clients. # Create a reentrant callback group to set the vehicle mode. vehicle_mode_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create vehicle state service client: {VEHICLE_STATE_SERVICE}") self.vehicle_state_cli = self.create_client(ActiveStateSrv, VEHICLE_STATE_SERVICE, callback_group=vehicle_mode_cb_group) self.wait_for_service_availability(self.vehicle_state_cli) # Create a reentrant callback group to activate the state. enable_state_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create enable state service client: {ENABLE_STATE_SERVICE}") self.enable_state_cli = self.create_client(EnableStateSrv, ENABLE_STATE_SERVICE, callback_group=enable_state_cb_group) self.wait_for_service_availability(self.enable_state_cli) # Create a reentrant callback group to get the calibration value. get_calibration_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get_cal service client: {GET_CAR_CAL_SERVICE}") self.get_cal_cli = self.create_client(GetCalibrationSrv, GET_CAR_CAL_SERVICE, callback_group=get_calibration_cb_group) self.wait_for_service_availability(self.get_cal_cli) # Create a mutually exclusive callback group to set the calibration value. set_calibration_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set_cal service client: {SET_CAR_CAL_SERVICE}") self.set_cal_cli = self.create_client(SetCalibrationSrv, SET_CAR_CAL_SERVICE, callback_group=set_calibration_cb_group) self.wait_for_service_availability(self.set_cal_cli) # Create a reentrant callback group to get the device info values. get_device_info_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create device info service client: {GET_DEVICE_INFO_SERVICE}") self.get_revision_info_cli = self.create_client(GetDeviceInfoSrv, GET_DEVICE_INFO_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.get_revision_info_cli) self.get_logger().info(f"Create battery level service client: {BATTERY_LEVEL_SERVICE}") self.battery_level_cli = self.create_client(BatteryLevelSrv, BATTERY_LEVEL_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.battery_level_cli) self.get_logger().info(f"Create sensor status service client: {SENSOR_DATA_STATUS_SERVICE}") self.sensor_status_cli = self.create_client(SensorStatusCheckSrv, SENSOR_DATA_STATUS_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.sensor_status_cli) # Create a mutually exclusive callback group to set the tail light LED color values. set_led_color_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set led color service client: {SET_CAR_LED_SERVICE}") self.set_led_color_cli = self.create_client(SetLedCtrlSrv, SET_CAR_LED_SERVICE, callback_group=set_led_color_cb_group) self.wait_for_service_availability(self.set_led_color_cli) # Create a reentrant callback group to set the tail light LED color values. get_led_color_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get led color service client: {GET_CAR_LED_SERVICE}") self.get_led_color_cli = self.create_client(GetLedCtrlSrv, GET_CAR_LED_SERVICE, callback_group=get_led_color_cb_group) self.wait_for_service_availability(self.get_led_color_cli) # Create a reentrant callback group to call load model services. model_load_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create verify model ready service client: {VERIFY_MODEL_READY_SERVICE}") self.verify_model_ready_cli = self.create_client(VerifyModelReadySrv, VERIFY_MODEL_READY_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.verify_model_ready_cli) self.get_logger().info(f"Create configure LiDAR service client: {CONFIGURE_LIDAR_SERVICE}") self.configure_lidar_cli = self.create_client(LidarConfigSrv, CONFIGURE_LIDAR_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.configure_lidar_cli) self.get_logger().info(f"Create model state service client: {MODEL_STATE_SERVICE}") self.model_state_cli = self.create_client(ModelStateSrv, MODEL_STATE_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.model_state_cli) # Create a reentrant callback group to call model loading status service. is_model_loading_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create is model loading service client: {IS_MODEL_LOADING_SERVICE}") self.is_model_loading_cli = self.create_client(GetModelLoadingStatusSrv, IS_MODEL_LOADING_SERVICE, callback_group=is_model_loading_cb_group) self.wait_for_service_availability(self.is_model_loading_cli) # Create a mutually exclusive callback group to call model action services. model_action_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create upload model service client: {CONSOLE_MODEL_ACTION_SERVICE}") self.model_action_cli = self.create_client(ConsoleModelActionSrv, CONSOLE_MODEL_ACTION_SERVICE, callback_group=model_action_cb_group) self.wait_for_service_availability(self.model_action_cli) # Create a reentrant callback group to call software update check service. sw_update_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update state check service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_state_cli = self.create_client(SoftwareUpdateCheckSrv, SOFTWARE_UPDATE_CHECK_SERVICE_NAME, callback_group=sw_update_state_cb_group) self.wait_for_service_availability(self.sw_update_state_cli) # Create a reentrant callback group to call software update services. sw_update_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create begin sw update service client: {BEGIN_UPDATE_SERVICE}") self.begin_sw_update_cli = self.create_client(BeginSoftwareUpdateSrv, BEGIN_UPDATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.begin_sw_update_cli) self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_status_cli = self.create_client(SoftwareUpdateStateSrv, SOFTWARE_UPDATE_STATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.sw_update_status_cli) # Create a reentrant exclusive callback group for the software progress subscriber. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.sw_update_pct_sub_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_STATE_SERVICE}") self.sw_update_pct_sub = self.create_subscription(SoftwareUpdatePctMsg, SOFTWARE_UPDATE_PCT_TOPIC, self.sw_update_pct_sub_cb, callback_group=self.sw_update_pct_sub_cb_group, qos_profile=qos_profile) # Create a reentrant callback group to call autonomous throttle update service. set_throttle_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create set throttle service client: " f"{AUTONOMOUS_THROTTLE_SERVICE}") self.set_throttle_cli = self.create_client(NavThrottleSrv, AUTONOMOUS_THROTTLE_SERVICE, callback_group=set_throttle_cb_group) self.wait_for_service_availability(self.set_throttle_cli) # Create a reentrant callback group to call Get Ctrl Modes service. get_ctrl_modes_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create Get Ctrl Modes service client: " f"{GET_CTRL_MODES_SERVICE}") self.get_ctrl_modes_cli = self.create_client(GetCtrlModesSrv, GET_CTRL_MODES_SERVICE, callback_group=get_ctrl_modes_cb_group) self.wait_for_service_availability(self.get_ctrl_modes_cli) # Create a reentrant callback group to call otg link state service. otg_link_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create otg link state service client: " f"{GET_OTG_LINK_STATE_SERVICE}") self.otg_link_state_cli = self.create_client(OTGLinkStateSrv, GET_OTG_LINK_STATE_SERVICE, callback_group=otg_link_state_cb_group) self.wait_for_service_availability(self.otg_link_state_cli) # Create a reentrant callback group to publish manual drive messages. manual_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create manual drive publisher: {MANUAL_DRIVE_TOPIC}") self.pub_manual_drive = self.create_publisher(ServoCtrlMsg, MANUAL_DRIVE_TOPIC, 1, callback_group=manual_pub_drive_msg_cb_group) # Create a reentrant callback group to publish calibration drive messages. cal_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create calibration drive publisher: {CAL_DRIVE_TOPIC}") self.pub_calibration_drive = self.create_publisher(ServoCtrlMsg, CAL_DRIVE_TOPIC, 1, callback_group=cal_pub_drive_msg_cb_group) # Double buffer object to safely updat the latest status and installation percentage # during software update setup. self.pct_dict_db = DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({"status": "unknown", "update_pct": 0.0}) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback)