def __init__(
self,
node: Node,
topic_and_expected_values_pairs: List[TopicAndValuesPair],
eqaulity_type: EqualityType,
callback: Callable[
[bool, Dict[str, bool], List[TopicAndValuesPair], EqualityType],
None] = default_multi_message_callback):
self.__equality_type = eqaulity_type
self.__callback = callback
self.__topic_and_expected_values_pairs = topic_and_expected_values_pairs
node.get_logger().info("Setting up subscriptions")
for topic_and_expected_values_pair in topic_and_expected_values_pairs:
#Create MessageEqualityTester that store the result in __message_store when a new message
#is received, and then perform the predicate check (which calls the callback)
def cb(val: bool,
actual_msg: Dict,
expected_values: Dict,
topic_and_expected_values_pair=topic_and_expected_values_pair
):
#Store the value of that message
self.__message_store[
topic_and_expected_values_pair.topic_name] = val
node.get_logger().info("Message received ({}/{})".format(
len(self.__message_store),
len(topic_and_expected_values_pairs)))
if (len(self.__message_store) == len(
topic_and_expected_values_pairs)):
node.get_logger().info(
"All messages received, performing check")
self.__perform_check()
tester = MessageEqualityTester(
node, topic_and_expected_values_pair.topic_name,
topic_and_expected_values_pair.topic_type,
topic_and_expected_values_pair.values, cb)
self.__testers.append(tester)
node.get_logger().info(
"Waiting for at least one message on each topic...")
def __init__(self,
node_name,
*,
enable_communication_interface: bool = True,
**kwargs):
"""
Create a lifecycle node.
See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node()
for the documentation of each parameter.
"""
Node.__init__(self, node_name, **kwargs)
LifecycleNodeMixin.__init__(
self,
enable_communication_interface=enable_communication_interface)
def __init__(self, name, planning_rate: float,
planner: AbstractPlannerImplementation):
self.name = name
Node.__init__(self, "planner_" + name)
# Abstract implementation of planner (adapter)
self.planner_adapter = planner
# Basic I/O interface (input and refined trajectory)
# Subscriber interface for trajectory planning
self._refined_trajectory = self.create_publisher(
Trajectory, '/'.join([self.name, 'refined_trajectory']), 1)
self._input_trajectory = None
# Planner timer cycle
self._plan_timer = self.create_timer(planning_rate,
self.cb_timer_planner)
self.get_logger().info("Starting planner node {0}".format(self.name))
def __init__(self):
# Initialise node
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
Node.__init__(self, "octree_creator_test")
self.__point_cloud_sub = self.create_subscription(
PointCloud2, "rgbd_camera/points", self.point_cloud_callback, 1)
self.octree_creator = OctreeCreator()
def timer_callback(self):
"""Timer Callback Function
This method captures images and publishes required data in ros 2 topic.
"""
if self.capture.isOpened():
# reads image data
ret, frame = self.capture.read()
# processes image data and converts to ros 2 message
msg = Image()
msg.header.stamp = Node.get_clock(self).now().to_msg()
msg.header.frame_id = 'ANI717'
msg.height = np.shape(frame)[0]
msg.width = np.shape(frame)[1]
msg.encoding = "bgr8"
msg.is_bigendian = False
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
msg.data = np.array(frame).tobytes()
# publishes message
self.publisher_.publish(msg)
self.get_logger().info('%d Images Published' % self.i)
# image counter increment
self.i += 1
return None
def main():
rclpy.init()
node = Node('table_parameters',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
N = node.get_parameter('N').value
table = PositionTaskTable(N)
table.gc.trigger()
table.get_logger().info('Waiting for 10 seconds to let all nodes be ready')
time.sleep(10)
rclpy.spin(table)
rclpy.shutdown()
def create_node(node_name: str,
*,
context: Context = None,
cli_args: List[str] = None,
namespace: str = None,
use_global_arguments: bool = True,
start_parameter_services: bool = True,
initial_parameters: List[Parameter] = None) -> 'Node':
"""
Create an instance of :class:`.Node`.
:param node_name: A name to give to the node.
:param context: The context to associated with the node, or ``None`` for the default global
context.
:param cli_args: Command line arguments to be used by the node.
:param namespace: The namespace prefix to apply to entities associated with the node
(node name, topics, etc).
:param use_global_arguments: ``False`` if the node should ignore process-wide command line
arguments.
:param start_parameter_services: ``False`` if the node should not create parameter services.
:param initial_parameters: A list of :class:`.Parameter` to be set during node creation.
:return: An instance of the newly created node.
"""
# imported locally to avoid loading extensions on module import
from rclpy.node import Node
return Node(node_name,
context=context,
cli_args=cli_args,
namespace=namespace,
use_global_arguments=use_global_arguments,
start_parameter_services=start_parameter_services,
initial_parameters=initial_parameters)
def __init__(self, node: Node, params: dict):
self._node = node
self.topic = params['topic']
self.lin_throttle = params['throttle']['lin_throttle_ctrl']
self.ang_throttle = params['throttle']['ang_throttle_ctrl']
self._is_turbo = False
self._before_turbo = 1.0
self.x = params['data']['linear']['x']
self.y = params['data']['linear']['y']
self.z = params['data']['linear']['z']
self.roll = params['data']['angular']['x']
self.pitch = params['data']['angular']['y']
self.yaw = params['data']['angular']['z']
self._HALT = Twist()
self._last_message_was_halt = False
self._lin_throttle_increment = params['throttle']['lin_increment']
self._ang_throttle_increment = params['throttle']['ang_increment']
self._last_lin_throttle_input = 0
self._lin_throttle_coef = 1.0
self._last_ang_throttle_input = 0
self._ang_throttle_coef = 1.0
try:
self.publish_multiple_halts = params['publish_multiple_halt']
except:
self.publish_multiple_halts = True
self._publisher = node.create_publisher(Twist, self.topic, 10)
def __init__(self, node: Node, joint_name: str, topic: str,
msg_type: type):
self.node = node
self.joint_name = joint_name
node.create_subscription(msg_type, topic, self._cb, qos_profile=10)
(
self._default_threshold,
self._thresholds_warning,
self._thresholds_non_fatal,
self._thresholds_fatal,
) = self._get_temperature_thresholds()
self._timestamp = None
self._temperature = None
def main():
rclpy.init()
node = Node('cartesian_compute')
try:
c = CartesianCompute(node)
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
return 0
def to_path_marker(self, node: Node, clear=False):
"""Return a `nav_msgs/Path` message with all waypoints in the set
> *Input arguments*
* `clear` (*type:* `bool`, *default:* `False`): Return an empty `nav_msgs/Path` message.
> *Returns*
`nav_msgs/Path` message
"""
path = Path()
t = node.get_clock().now()
path.header.stamp = t.to_msg()
path.header.frame_id = self._inertial_frame_id
if self.num_waypoints > 1 and not clear:
for i in range(self.num_waypoints):
wp = self.get_waypoint(i)
pose = PoseStamped()
pose.header.stamp = rclpy.time.Time(seconds=i) # TODO check
pose.header.frame_id = self._inertial_frame_id
pose.pose.position.x = wp.x
pose.pose.position.y = wp.y
pose.pose.position.z = wp.z
path.poses.append(pose)
return path
def create_node(node_name,
*,
context=None,
cli_args=None,
namespace=None,
use_global_arguments=True,
start_parameter_services=True,
initial_parameters=None):
"""
Create an instance of :class:`rclpy.node.Node`.
:param node_name: A unique name to give to this node.
:param context: The context to be associated with, or None for the default global context.
:param cli_args: A list of strings of command line args to be used only by this node.
:param namespace: The namespace to which relative topic and service names will be prefixed.
:param use_global_arguments: False if the node should ignore process-wide command line args.
:param start_parameter_services: False if the node should not create parameter services.
:param initial_parameters: A list of rclpy.parameter.Parameters to be set during node creation.
:return: An instance of a node
:rtype: :class:`rclpy.node.Node`
"""
# imported locally to avoid loading extensions on module import
from rclpy.node import Node
return Node(node_name,
context=context,
cli_args=cli_args,
namespace=namespace,
use_global_arguments=use_global_arguments,
start_parameter_services=start_parameter_services,
initial_parameters=initial_parameters)
def test_declare_qos_parameters():
node = Node('my_node')
_declare_qos_parameters(Publisher, node, '/my_topic', QoSProfile(depth=10),
QoSOverridingOptions.with_default_policies())
qos_overrides = node.get_parameters_by_prefix('qos_overrides')
assert len(qos_overrides) == 3
expected_params = (
('/my_topic.publisher.depth', 10),
('/my_topic.publisher.history', 'keep_last'),
('/my_topic.publisher.reliability', 'reliable'),
)
for actual, expected in zip(
sorted(qos_overrides.items(), key=lambda x: x[0]),
expected_params):
assert actual[0] == expected[0] # same param name
assert actual[1].value == expected[1] # same param value
def main():
rclpy.init()
node = Node('Controller')
pub = node.create_publisher(String, 'DIRECTION', 10)
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
msg = String()
while ip != 'Z':
if (ip == "W" or ip == "S" or ip == "A" or ip == "D" or ip == "Q"):
msg.data = ip
pub.publish(msg)
else:
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
node.destroy_node()
rclpy.shutdown()
def __init__(self, server_name, state, path):
"""Traverse the smach tree starting at root, and construct introspection
proxies for getting and setting debug state."""
Node.__init__(self, server_name)
# A list of introspection proxies
self._proxies = []
# Store args
self._server_name = server_name
self._state = state
self._path = path
self._executor = SingleThreadedExecutor()
self._executor.add_node(self)
self._spinner = threading.Thread(target=self._executor.spin)
self._spinner.start()
def __init__(self,origion_x=0.0,origion_y=0.0,resolution=0.1, width=50, height=50):
super().__init__('map_pub')
self.msg = OccupancyGrid()
#self.pub = self.create_publisher(OccupancyGrid)
#timer_period = 0.05
#self.timer = self.create_timer(timer_period, self.scan)
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
self.orogion_x = origion_x
self.origion_y = origion_y
self.resolution = resolution
self.width = width
self.height = height
self.grid = np.zeros((height, width))
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
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)."""
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)
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)
def main(args=None):
rclpy.init(args=args)
node = Node("task_power_rune2019_client")
cli = node.create_client(SetMode, "task_power_rune/set_mode")
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
print(msg)
old_attr = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin.fileno())
while rclpy.ok():
if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
key=sys.stdin.read(1)
print(key)
if (key == 'q' or key == 'Q'):
set_mode_req(cli,0x00)
elif(key=='w' or key=='W'):
set_mode_req(cli,0x01)
elif(key=='e' or key=='E'):
set_mode_req(cli,0x02)
elif(key=='r' or key=='R'):
set_mode_req(cli,0x03)
elif(key=='a' or key=='A'):
set_mode_req(cli,0x10)
elif(key=='s' or key=='S'):
set_mode_req(cli,0x11)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr)
node.destroy_node()
rclpy.shutdown()
def wait_for_message(self, params, msg_type=msg_Image):
topic = params['topic']
print ('connect to ROS with name: %s' % self.node_name)
rclpy.init()
node = Node(self.node_name)
out_filename = params.get('filename', None)
if (out_filename):
self.fout = open(out_filename, 'w')
if msg_type is msg_Imu:
col_w = 20
print ('Writing to file: %s' % out_filename)
columns = ['frame_number', 'frame_time(sec)', 'accel.x', 'accel.y', 'accel.z', 'gyro.x', 'gyro.y', 'gyro.z']
line = ('{:<%d}'*len(columns) % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(*columns) + '\n'
sys.stdout.write(line)
self.fout.write(line)
node.get_logger().info('Subscribing on topic: %s' % topic)
sub = node.create_subscription(msg_type, topic, self.callback, qos.qos_profile_sensor_data)
self.prev_time = time.time()
break_timeout = False
while not any([(not rclpy.ok()), break_timeout, self.result]):
rclpy.spin_once(node)
if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
break_timeout = True
node.destroy_subscription(sub)
return self.result
def wait_for_messages(self, themes):
# tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}}
self.func_data = dict([[theme_name, {}] for theme_name in themes])
node = Node('wait_for_messages')
for theme_name in themes:
theme = self.themes[theme_name]
node.get_logger().info('Subscribing %s on topic: %s' %
(theme_name, theme['topic']))
self.func_data[theme_name]['sub'] = node.create_subscription(
theme['msg_type'], theme['topic'],
theme['callback'](theme_name), qos.qos_profile_sensor_data)
self.tfBuffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node)
self.prev_time = time.time()
break_timeout = False
while not break_timeout:
rclpy.spin_once(node, timeout_sec=1)
if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
break_timeout = True
self.unregister_all(node, self.func_data)
node.destroy_node()
return self.func_data
def test_destruction_order():
context = rclpy.context.Context()
rclpy.init(context=context)
node1 = Node('test_destruction_order_node_1', context=context)
node2 = Node('test_destruction_order_node_2', context=context)
node3 = Node('test_destruction_order_node_3', context=context)
node1.cyclic_ref = node1
node2.cyclic_ref = node2
node1.node3 = node3
node2.node3 = node3
node1.handle.context_handle = context.handle
node2.handle.context_handle = context.handle
def rosinit(self):
node_name = 'circle'
rclpy.init()
node = Node(node_name)
msg = Float32()
print('Spinning: {}'.format(node_name))
self.publeft = node.create_publisher(Float32, '/wheel_left')
self.pubright = node.create_publisher(Float32, '/wheel_right')
if self.problem == 'triangle':
self.goal_pt = next(self.tri_pts)
self.goal_pt = next(self.tri_pts)
self.goal_theta = next(self.tri_thetas)
self.goal_theta = next(self.tri_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.tri_callback)
elif self.problem == 'square':
self.goal_pt = next(self.sqr_pts)
self.goal_pt = next(self.sqr_pts)
self.goal_theta = next(self.sqr_thetas)
self.goal_theta = next(self.sqr_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.sqr_callback)
else:
# Circle
msg.data = 5.0
self.publeft.publish(msg)
msg.data = msg.data * (15.0 + 1.2) / 15
self.pubright.publish(msg)
try:
rclpy.spin(node)
except KeyboardInterrupt:
print('Shutting down...')
msg.data = 0.0
self.publeft.publish(msg)
self.pubright.publish(msg)
node.destroy_node()
rclpy.shutdown()
def __init__(self):
Node.__init__(self, "pgcd_comp",
allow_undeclared_parameters = True,
automatically_declare_parameters_from_overrides = True)
Interpreter.__init__(self, self.get_name())
TFUpdater.__init__(self)
# program
self.id = self.get_name()
self.prog_path = self.get_parameter('program_location')._value + self.id + '.rosl'
rclpy.logging._root_logger.log("PGCD creating " + self.id + " running " + self.prog_path, LoggingSeverity.INFO)
# hardware
module = importlib.import_module(self.get_parameter('object_module_name')._value)
class_ = getattr(module, self.get_parameter('object_class_name')._value)
self.robot = class_()
# runtime
self.tfBuffer = tf2_ros.Buffer()
self.tf2_listener = tf2_ros.TransformListener(self.tfBuffer, self)
self.tf2_setup(self.robot)
def __init__(self):
super().__init__('pose_estimate')
self.msg = geometry_msgs.msg.TransformStamped()
self.pub = self.create_publisher(geometry_msgs.msg.TransformStamped,
"/tf", 1)
timer_period = 0.05
self.timer = self.create_timer(timer_period, self.scan)
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
def init(self, node: Node, yellow_: bool) -> None:
self.node_ = node
self.yellow_ = yellow_
# Robots
self.p_allies_ = node.create_subscription(Robots, 'allies',
self.update_allies, 10)
self.p_opponents_ = node.create_subscription(Robots, 'opponents',
self.update_opponents, 10)
# Ball
self.p_ball_ = node.create_subscription(Ball, 'ball', self.update_ball,
10)
# GameController
self.p_gc_ = node.create_subscription(Referee, 'gc', self.update_gc,
10)
def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
# The logic for responding to this joystick press is:
# 1. Save off the current state of active.
# 2. Update the current state of active based on buttons and axes.
# 3. If this command is currently not active, return without publishing.
# 4. If this is a msg_value, and the value of the previous active is the same as now,
# debounce and return without publishing.
# 5. In all other cases, publish. This means that this is a msg_value and the button
# transitioned from 0 -> 1, or it means that this is an axis mapping and data should
# continue to be published without debouncing.
last_active = self.active
self.update_active_from_buttons_and_axes(joy_state)
if not self.active:
return
if self.msg_value is not None and last_active == self.active:
return
if self.msg_value is not None:
# This is the case for a static message.
msg = self.msg_value
else:
# This is the case to forward along mappings.
msg = self.topic_type()
for mapping, values in self.axis_mappings.items():
if 'axis' in values:
if len(joy_state.axes) > values['axis']:
val = joy_state.axes[values['axis']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} axes (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.axes), values['axis']))
val = 0.0
elif 'button' in values:
if len(joy_state.buttons) > values['button']:
val = joy_state.buttons[values['button']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} buttons (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.buttons), values['button']))
val = 0.0
elif 'value' in values:
# Pass on the value as its Python-implicit type
val = values.get('value')
else:
node.get_logger().error(
'No Supported axis_mappings type found in: {}'.format(mapping))
val = 0.0
set_member(msg, mapping, val)
# If there is a stamp field, fill it with now().
if hasattr(msg, 'header'):
msg.header.stamp = node.get_clock().now().to_msg()
self.pub.publish(msg)
def __init__(self,
topic: str,
is_point_cloud: bool,
use_sim_time: bool = True,
node_name: str = 'drl_grasping_camera_sub'):
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
Node.__init__(self, node_name)
self.set_parameters([
Parameter('use_sim_time',
type_=Parameter.Type.BOOL,
value=use_sim_time)
])
# Prepare the subscriber
if is_point_cloud:
observation_type = PointCloud2
else:
observation_type = Image
self.__observation = observation_type()
self.__observation_sub = self.create_subscription(
msg_type=observation_type,
topic=topic,
callback=self.observation_callback,
qos_profile=QoSProfile(
durability=QoSDurabilityPolicy.SYSTEM_DEFAULT,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
history=QoSHistoryPolicy.SYSTEM_DEFAULT))
self.__observation_mutex = Lock()
self.__new_observation_available = False
# Spin the node in a separate thread
self._executor = SingleThreadedExecutor()
self._executor.add_node(self)
self._executor_thread = Thread(target=self._executor.spin,
args=(),
daemon=True)
self._executor_thread.start()
def __init__(self, node:Node):
self.node = node
self.shut_down_request = False
self.last_initialized = None
self.initialized = False
self.use_sim_time = self.node.get_parameter('use_sim_time').value
# we only need tf in simulation. don't use it otherwise to safe performance
if self.use_sim_time:
self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=10))
self.tf_listener = tf2.TransformListener(self.tf_buffer, node)
self.odom_frame = node.get_parameter('odom_frame').get_parameter_value().string_value
self.base_footprint_frame = node.get_parameter('base_footprint_frame').get_parameter_value().string_value
self.field_length = node.get_parameter('field_length').get_parameter_value().double_value
# services
self.reset_filter_proxy = node.create_client(ResetFilter, 'reset_localization')
self.stop_filter_proxy = node.create_client(SetPaused, 'pause_localization')
# Pose
self.last_pose_update_time = None
self.poseX = 0
self.poseY = 0
self.orientation = np.array([0, 0, 0, 1])
self.covariance = np.array([])
#GameState
self.gamestate = GameStatusCapsule(node)
#Robot Control State
self.robot_control_state = None
self.last_robot_control_state = None
#Get up
self.last_state_get_up = False
#Picked up
self.last_state_pickup = False
#Last init action
self.last_init_action_type = False
self.last_init_odom_transform = None
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 = import_message_type(topic_name, 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)
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 __init__(self):
rclpy.init(args=None)
self.node = Node("Pause")
self.penalty_manual = False
self.game_controller_penalty = False
self.pause = False
self.manual_penalize_service = self.node.create_service(
ManualPenalize, "manual_penalize", self.manual_update)
self.pause_publisher = self.node.create_publisher(Bool, "pause",
10) # todo latch
self.speak_publisher = self.node.create_publisher(Audio, "speak", 10)
while rclpy.ok():
try:
rclpy.spin_once(self.node)
except (ExternalShutdownException, KeyboardInterrupt):
exit(0)
