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)
