def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, times: int, wait_matching_subscriptions: int, qos_profile: QoSProfile, keep_alive: float, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" try: msg_module = get_message(message_type) except (AttributeError, ModuleNotFoundError, ValueError): raise RuntimeError('The passed message type is invalid') values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' pub = node.create_publisher(msg_module, topic_name, qos_profile) times_since_last_log = 0 while pub.get_subscription_count() < wait_matching_subscriptions: # Print a message reporting we're waiting each 1s, check condition each 100ms. if not times_since_last_log: print( f'Waiting for at least {wait_matching_subscriptions} matching subscription(s)...' ) times_since_last_log = (times_since_last_log + 1) % 10 time.sleep(0.1) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer_callback() if times != 1: timer = node.create_timer(period, timer_callback) while times == 0 or count < times: rclpy.spin_once(node) # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) node.destroy_timer(timer) else: # give some time for the messages to reach the wire before exiting time.sleep(keep_alive)
def main(args=None): rclpy.init(args=None) # needed to init rclcpp ros for moveit_bindings node = Node("bitbots_localization_handler", automatically_declare_parameters_from_overrides=True) dsd = init(node) node.create_timer(1 / 25.0, dsd.update) multi_executor = MultiThreadedExecutor() multi_executor.add_node(node) try: multi_executor.spin() except KeyboardInterrupt: pass node.destroy_node()
def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, once: bool, qos_profile: QoSProfile, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" msg_module = get_message(message_type) values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' publisher_callbacks = PublisherEventCallbacks( incompatible_qos=handle_incompatible_qos_event) try: pub = node.create_publisher(msg_module, topic_name, qos_profile, event_callbacks=publisher_callbacks) except UnsupportedEventTypeError: pub = node.create_publisher(msg_module, topic_name, qos_profile) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer = node.create_timer(period, timer_callback) if once: rclpy.spin_once(node) time.sleep( 0.1) # make sure the message reaches the wire before exiting else: rclpy.spin(node) node.destroy_timer(timer)
def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, times: int, qos_profile: QoSProfile, keep_alive: float, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" msg_module = get_message(message_type) values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' pub = node.create_publisher(msg_module, topic_name, qos_profile) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer = node.create_timer(period, timer_callback) while times == 0 or count < times: rclpy.spin_once(node) # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) node.destroy_timer(timer)
import rclpy from rclpy.node import Node from std_msgs.msg import Int16 rclpy.init() node = Node("talker") pub = node.create_publisher(Int16, "countup", 10) n = 0 def cb(): global n msg = Int16() msg.data = n pub.publish(msg) n += 1 timer = node.create_timer(0.5, cb) rclpy.spin(node)
// SPDX-License-Identifier: BSD-3.0 /* Copyright (C) 2020 Yuka Matsuura and Ryuichi Ueda. All right reserved. */ import rclpy from rclpy.node import Node from std_msgs.msg import Float32 rclpy.init() node= Node("talker") #ノード作成 pub = node.create_publisher(Float32, "BMI", 10) #パブリッシャ作成 def BMI(): global weight global height msg = Float32() msg2 = Float32() weight = float(input("How much do yo weight?(Kg)")) height = float(input("How tall are you?(m)")) msg.data = weight msg2.data = height pub.publish(msg) pub.publish(msg2) node.create_timer(0.5, BMI) #タイマー設定 rclpy.spin(node) #実行
import rclpy from rclpy.node import Node from std_msgs.msg import Int16 rclpy.init() node = Node("talker") #ノード作成 pub = node.create_publisher(Int16, "countup", 10) #パブリッシャ作成 n = 0 #カウント用変数 def cb(): #定期的に呼ぶコールバック関数 global n msg = Int16() msg.data = n pub.publish(msg) n += 1 node.create_timer(0.5, cb) #タイマー設定(周期0.5[s]) rclpy.spin(node) #実行(無限ループ)
import rclpy from rclpy.node import Node from std_msgs.msg import Int16 rclpy.init() node = Node("talker") pub = node.create_publisher(Int16, "countup", 10) n = 0 def cb(): global n msg = Int16() msg.data = n pub.publish(msg) n += 1 node.create_timer(1.5, cb) rclpy.spin(node)
} node.get_logger().warn( f"Namespace not specified, use '__ns:=/XXXX' argument for specific namespace" ) print("Discovered namespaces:") for i, ns in ns_selection.items(): print(f"\t{i}: {ns}") try: selection = input("Please enter desired namespace number: ") except KeyboardInterrupt: node.destroy_node() sys.exit(0) if int(selection) not in ns_selection.keys(): print("WRONG") node.destroy_node() sys.exit(0) ns = ns_selection[int(selection)] print(f"Restarting in namespace: {ns}") node.destroy_node() node = Node("diag_listener", namespace=ns) sub = node.create_subscription(diag_msg, "diagnostics", diag_cb, 10) timer = node.create_timer(2, timer_cb) timer.reset() print(f"Listening for {str(type(diag_msg))} from [{frame_id}]") while rclpy.ok(): rclpy.spin_once(node, timeout_sec=0.2) node.destroy_node() rclpy.shutdown()
class Ui(QtWidgets.QMainWindow): def __init__(self): super(Ui, self).__init__() # Initialize UI self.load_ui() self.reset_ui_state() # Init state self.recording_enabled_target = False # Create ROS self.node = Node("asv_status") # Init ROS Entities self.init_ros() # Create Qt timer to poll ROS2 node for events self.ros_timer = QtCore.QTimer(self) self.ros_timer.timeout.connect(self.handle_ros) self.ros_timer.start(10) # Connect Checkbox signal to publish ROS message with updated state self.checkbox_recording_enabled.stateChanged.connect( self.update_recording_enable_target) # Display window self.show() def update_recording_enable_target(self, state): if (QtCore.Qt.Checked == state): self.recording_enabled_target = True else: self.recording_enabled_target = False msg = Bool() msg.data = self.recording_enabled_target self.pub_req_enable_recording.publish(msg) def init_ros(self): # Create ROS Timer for vehicle heartbeat self.heartbeat_received = False self.last_heartbeat_time = datetime.datetime.now() self.heartbeat_timer = self.node.create_timer(1, self.heartbeat_callback) # Create QOS profiles qos_be = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) qos_klr_v = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) qos_klr_tl = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) # Create Subscribers self.sub_heartbeat = self.node.create_subscription( msg_type=String, topic='heartbeat', callback=self.update_heartbeat, qos_profile=qos_klr_v) self.sub_camera_status = self.node.create_subscription( msg_type=String, topic='camera_status', callback=self.update_camera_status, qos_profile=qos_klr_tl) self.sub_gps_status = self.node.create_subscription( msg_type=String, topic='gps_status', callback=self.update_gps_status, qos_profile=qos_klr_tl) self.sub_storage_status = self.node.create_subscription( msg_type=String, topic='storage_status', callback=self.update_storage_status, qos_profile=qos_klr_tl) self.sub_system_time = self.node.create_subscription( msg_type=String, topic='system_time', callback=self.update_system_time, qos_profile=qos_klr_tl) self.sub_recording_status = self.node.create_subscription( msg_type=String, topic='recording_status', callback=self.update_recording_status, qos_profile=qos_klr_tl) self.sub_recorded_samples = self.node.create_subscription( msg_type=String, topic='recorded_samples', callback=self.update_recorded_samples, qos_profile=qos_klr_tl) self.sub_video_left = self.node.create_subscription( msg_type=CompressedImage, topic='camera_left/image_raw/compressed', callback=self.update_video_image, qos_profile=qos_be) # Create publisher self.pub_req_enable_recording = self.node.create_publisher( msg_type=Bool, topic='recording_enabled_target', qos_profile=qos_klr_tl) # Set initial recording enable target msg = Bool() msg.data = self.recording_enabled_target self.pub_req_enable_recording.publish(msg) def load_ui(self): # Get path to Qt UI file package_share_directory = get_package_share_directory('asv_status') ui_path = os.path.join(package_share_directory, "ui/asv_status.ui") # Load UI uic.loadUi(ui_path, self) # Find UI elements self.label_camera_status = self.findChild(QtWidgets.QLabel, 'label_camera_status') self.label_gps_status = self.findChild(QtWidgets.QLabel, 'label_gps_status') self.label_storage_status = self.findChild(QtWidgets.QLabel, 'label_storage_status') self.label_system_time = self.findChild(QtWidgets.QLabel, 'label_system_time') self.label_recording_status = self.findChild(QtWidgets.QLabel, 'label_recording_status') self.label_recorded_samples = self.findChild(QtWidgets.QLabel, 'label_recorded_samples') self.label_video = self.findChild(QtWidgets.QLabel, 'label_video') self.checkbox_recording_enabled = self.findChild( QtWidgets.QCheckBox, 'checkbox_recording_enabled') def reset_ui_state(self): # Set starting values for labels self.update_camera_status(String(data="UNKNOWN")) self.update_gps_status(String(data="UNKNOWN")) self.update_storage_status(String(data="UNKNOWN")) self.update_system_time(String(data="UNKNOWN")) self.update_recording_status(String(data="NOT READY")) self.update_recorded_samples(String(data="0")) # Reset video label self.label_video.clear() self.label_video.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignBottom) self.label_video.setScaledContents(True) self.label_video.setText("DISCONNECTED") def cleanup(self): self.node.destroy_node() def handle_ros(self): rclpy.spin_once(node=self.node, timeout_sec=0) def heartbeat_callback(self): # If we haven't received a new heartbeat message in the last 3 seconds, clear the video feed elapsed_ms = (datetime.datetime.now() - self.last_heartbeat_time).total_seconds() * 1000 if (elapsed_ms > 3000): self.last_heartbeat_time = datetime.datetime.now() if self.heartbeat_received is False: self.label_video.clear() self.label_video.setText("DISCONNECTED") else: self.label_video.setText("") self.heartbeat_received = False # Message handlers def update_camera_status(self, msg): self.camera_status = msg.data self.label_camera_status.setText(msg.data) def update_gps_status(self, msg): self.gps_status = msg.data self.label_gps_status.setText(msg.data) def update_storage_status(self, msg): self.storage_status = msg.data self.label_storage_status.setText(msg.data) def update_system_time(self, msg): self.system_time = msg.data self.label_system_time.setText(msg.data) def update_recording_status(self, msg): self.recording_status = msg.data self.label_recording_status.setText(msg.data) def update_recorded_samples(self, msg): self.recorded_samples = msg.data self.label_recorded_samples.setText(msg.data) def update_heartbeat(self, msg): self.heartbeat_received = True def update_video_image(self, msg): qp = QPixmap() if qp.loadFromData(msg.data): self.label_video.setPixmap(qp)
import rclpy from rclpy.node import Node from std_msgs.msg import Int16 rclpy.init() node = Node("talker") pub = node.create_publisher(Int16, "countup", 10) n = 0 def cb(): global n msg = Int16() msg.data = n pub.publish(msg) n += 1 node.create_timer(1, cb) rclpy.spin(node)