def test_time_source_using_sim_time(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
# Setting ROS time active on a time source should also cause attached clocks' use of ROS
# time to be set to active.
self.assertFalse(time_source.ros_time_is_active)
self.assertFalse(clock.ros_time_is_active)
time_source.ros_time_is_active = True
self.assertTrue(time_source.ros_time_is_active)
self.assertTrue(clock.ros_time_is_active)
# A subscriber should have been created
assert time_source._clock_sub is not None
# When using sim time, ROS time should look like the messages received on /clock
self.publish_clock_messages()
assert clock.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME)
assert clock.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME)
# Check that attached clocks get the cached message
clock2 = Clock(clock_type=ClockType.ROS_TIME)
time_source.attach_clock(clock2)
assert clock2.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME)
assert clock2.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME)
# Check detaching the node
time_source.detach_node()
node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy')
time_source.attach_node(node2)
node2.destroy_node()
assert time_source._node == node2
assert time_source._clock_sub is not None
def test_clock_change(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
time_source.ros_time_is_active = True
pre_cb = Mock()
post_cb = Mock()
threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
handler = clock.create_jump_callback(
threshold, pre_callback=pre_cb, post_callback=post_cb)
time_source.ros_time_is_active = False
pre_cb.assert_called()
post_cb.assert_called()
assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED
pre_cb.reset_mock()
post_cb.reset_mock()
time_source.ros_time_is_active = True
pre_cb.assert_called()
post_cb.assert_called()
assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_ACTIVATED
handler.unregister()
def test_sleep_for_ros_time_enabled(default_context):
clock = ROSClock()
clock._set_ros_time_is_active(True)
start_time = Time(seconds=1, clock_type=ClockType.ROS_TIME)
sleep_duration = Duration(seconds=10)
stop_time = start_time + sleep_duration
clock.set_ros_time_override(start_time)
retval = None
def run():
nonlocal retval
retval = clock.sleep_for(sleep_duration)
t = threading.Thread(target=run)
t.start()
# wait for thread to get inside sleep_for call
time.sleep(0.2)
clock.set_ros_time_override(stop_time)
# wait for thread to exit
start = clock.now()
t.join()
stop = clock.now()
assert stop - start < A_SMALL_AMOUNT_OF_TIME
assert retval
def __init__(
self, node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True,
start_parameter_services=True, initial_parameters=None
):
self._handle = None
self._context = get_default_context() if context is None else context
self._parameters = {}
self.publishers = []
self.subscriptions = []
self.clients = []
self.services = []
self.timers = []
self.guards = []
self.waitables = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self._handle = _rclpy.rclpy_create_node(
node_name, namespace, self._context.handle, cli_args, use_global_arguments)
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle))
# Clock that has support for ROS time.
# TODO(dhood): use sim time if parameter has been set on the node.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events)
node_parameters = _rclpy.rclpy_get_node_parameters(Parameter, self.handle)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
node_parameters.update({p.name: p for p in initial_parameters})
self.set_parameters_atomically(node_parameters.values())
if start_parameter_services:
self._parameter_service = ParameterService(self)
def test_triggered_clock_change_callbacks(self):
one_second = Duration(seconds=1)
negative_one_second = Duration(seconds=-1)
threshold1 = JumpThreshold(min_forward=one_second,
min_backward=negative_one_second,
on_clock_change=False)
threshold2 = JumpThreshold(min_forward=None,
min_backward=None,
on_clock_change=True)
threshold3 = JumpThreshold(min_forward=one_second,
min_backward=negative_one_second,
on_clock_change=True)
pre_callback1 = Mock()
post_callback1 = Mock()
pre_callback2 = Mock()
post_callback2 = Mock()
pre_callback3 = Mock()
post_callback3 = Mock()
clock = ROSClock()
handler1 = clock.create_jump_callback(threshold1,
pre_callback=pre_callback1,
post_callback=post_callback1)
handler2 = clock.create_jump_callback(threshold2,
pre_callback=pre_callback2,
post_callback=post_callback2)
handler3 = clock.create_jump_callback(threshold3,
pre_callback=pre_callback3,
post_callback=post_callback3)
clock._set_ros_time_is_active(True)
pre_callback1.assert_not_called()
post_callback1.assert_not_called()
pre_callback2.assert_called()
post_callback2.assert_called()
pre_callback3.assert_called()
post_callback3.assert_called()
pre_callback1.reset_mock()
post_callback1.reset_mock()
pre_callback2.reset_mock()
post_callback2.reset_mock()
pre_callback3.reset_mock()
post_callback3.reset_mock()
clock._set_ros_time_is_active(True)
pre_callback1.assert_not_called()
post_callback1.assert_not_called()
pre_callback2.assert_not_called()
post_callback2.assert_not_called()
pre_callback3.assert_not_called()
post_callback3.assert_not_called()
handler1.unregister()
handler2.unregister()
handler3.unregister()
def test_no_post_callback(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
time_source.ros_time_is_active = True
pre_cb = Mock()
threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
handler = clock.create_jump_callback(
threshold, pre_callback=pre_cb, post_callback=None)
time_source.ros_time_is_active = False
pre_cb.assert_called_once()
handler.unregister()
def test_time_source_not_using_sim_time(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
# When not using sim time, ROS time should look like system time
now = clock.now()
system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now()
assert (system_now.nanoseconds - now.nanoseconds) < 1e9
# Presence of clock publisher should not affect the clock
self.publish_clock_messages()
self.assertFalse(clock.ros_time_is_active)
now = clock.now()
system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now()
assert (system_now.nanoseconds - now.nanoseconds) < 1e9
# Whether or not an attached clock is using ROS time should be determined by the time
# source managing it.
self.assertFalse(time_source.ros_time_is_active)
clock2 = ROSClock()
clock2._set_ros_time_is_active(True)
time_source.attach_clock(clock2)
self.assertFalse(clock2.ros_time_is_active)
assert time_source._clock_sub is None
def test_sleep_for_ros_time_toggled(default_context, ros_time_enabled):
clock = ROSClock()
clock._set_ros_time_is_active(not ros_time_enabled)
retval = None
def run():
nonlocal retval
retval = clock.sleep_for(Duration(seconds=10))
t = threading.Thread(target=run)
t.start()
# wait for thread to get inside sleep_for call
time.sleep(0.2)
clock._set_ros_time_is_active(ros_time_enabled)
# wait for thread to exit
start = clock.now()
t.join()
stop = clock.now()
assert stop - start < A_SMALL_AMOUNT_OF_TIME
assert retval is False
def test_no_pre_callback(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
assert self.set_use_sim_time_parameter(True)
post_cb = Mock()
threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
handler = clock.create_jump_callback(
threshold, pre_callback=None, post_callback=post_cb)
assert self.set_use_sim_time_parameter(False)
post_cb.assert_called_once()
assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED
handler.unregister()
def test_log_throttle_ros_clock(self):
message_was_logged = []
ros_clock = ROSClock()
time_source = TimeSource()
time_source.attach_clock(ros_clock)
time_source.ros_time_is_active = True
for i in range(5):
message_was_logged.append(
rclpy.logging._root_logger.log(
'message_' + inspect.stack()[0][3] + '_' + str(i),
LoggingSeverity.INFO,
throttle_duration_sec=1,
throttle_time_source_type=ros_clock,
))
time.sleep(0.4)
self.assertEqual(
message_was_logged,
[
False, # t=0, throttled
False, # t=0.4, throttled
False, # t=0.8, throttled
False, # t=1.2, throttled
False # t=1.6, throttled
])
message_was_logged = []
for i in range(5):
message_was_logged.append(
rclpy.logging._root_logger.log(
'message_' + inspect.stack()[0][3] + '_' + str(i),
LoggingSeverity.INFO,
throttle_duration_sec=2,
throttle_time_source_type=ros_clock,
))
ros_clock.set_ros_time_override(
Time(
seconds=i + 1,
nanoseconds=0,
clock_type=ros_clock.clock_type,
))
self.assertEqual(
message_was_logged,
[
False, # t=0, throttled
False, # t=1.0, throttled
True, # t=2.0, not throttled
False, # t=3.0, throttled
True # t=4.0, not throttled
])
def test_headerless(self):
sub = Subscriber(self.node, String, "/empty")
cache = Cache(sub, 5, allow_headerless=False)
msg = String()
cache.add(msg)
self.assertIsNone(
cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)),
"Headerless message invalidly added.")
cache = Cache(sub, 5, allow_headerless=True)
cache.add(msg)
s = cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME))
self.assertEqual(s, msg, "invalid msg returned in headerless scenario")
currentRosTime = ROSClock().now()
s = cache.getElemAfterTime(currentRosTime)
self.assertIsNone(s, "invalid msg returned in headerless scenario")
cache.add(msg)
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
currentRosTime)
self.assertEqual(s, [msg],
"invalid msg returned in headerless scenario")
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
(currentRosTime + Duration(seconds=2)))
self.assertEqual(s, [msg, msg],
"invalid msg returned in headerless scenario")
def _publish_structure(self, info_str=''):
path = self._path
children = list(self._container.get_children().keys())
internal_outcomes = []
outcomes_from = []
outcomes_to = []
for (outcome, from_label,
to_label) in self._container.get_internal_edges():
internal_outcomes.append(str(outcome))
outcomes_from.append(str(from_label))
outcomes_to.append(str(to_label))
container_outcomes = self._container.get_registered_outcomes()
# Construct structure message
structure_msg = SmachContainerStructure(
header=Header(stamp=ROSClock().now().to_msg()),
path=path,
children=children,
internal_outcomes=internal_outcomes,
outcomes_from=outcomes_from,
outcomes_to=outcomes_to,
container_outcomes=container_outcomes)
try:
self._structure_pub.publish(structure_msg)
except:
if rclpy.ok():
self._server_node.get_logger().error(
"Publishing SMACH introspection structure message failed.")
def _to_object_inst(msg, rostype, roottype, inst, stack):
# Typecheck the msg
if type(msg) is not dict:
raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
# Substitute the correct time if we're an std_msgs/Header
if rostype in ros_header_types:
inst.stamp = ROSClock().now().to_msg()
inst_fields = inst.get_fields_and_field_types()
for field_name in msg:
# Add this field to the field stack
field_stack = stack + [field_name]
# Raise an exception if the msg contains a bad field
if not field_name in inst_fields:
raise NonexistentFieldException(roottype, field_stack)
field_rostype = inst_fields[field_name]
field_inst = getattr(inst, field_name)
field_value = _to_inst(msg[field_name], field_rostype, roottype,
field_inst, field_stack)
setattr(inst, field_name, field_value)
return inst
def add_client(self, client_id, ip_address):
with self._lock:
client = ConnectedClient()
client.ip_address = ip_address
client.connection_time = ROSClock().now().to_msg()
self._clients[client_id] = client
self.__publish()
def add(self, msg):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_cache')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn("can not use message filters messages "
"without timestamp infomation when "
"'allow_headerless' is disabled. "
"auto assign ROSTIME to headerless "
"messages once enabling constructor "
"option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# Insert sorted
self.cache_times.append(stamp)
self.cache_msgs.append(msg)
# Implement a ring buffer, discard older if oversized
if (len(self.cache_msgs) > self.cache_size):
del self.cache_msgs[0]
del self.cache_times[0]
# Signal new input
self.signalMessage(msg)
def add(self, msg, my_queue, my_queue_index=None):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_approx')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn(
"can not use message filters for "
"messages without timestamp infomation when "
"'allow_headerless' is disabled. auto assign "
"ROSTIME to headerless messages once enabling "
"constructor option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# print(stamp)
self.lock.acquire()
my_queue[stamp.nanoseconds] = msg
while len(my_queue) > self.queue_size:
del my_queue[min(my_queue)]
# self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...]
if my_queue_index is None:
search_queues = self.queues
else:
search_queues = self.queues[:my_queue_index] + \
self.queues[my_queue_index+1:]
# sort and leave only reasonable stamps for synchronization
stamps = []
for queue in search_queues:
topic_stamps = []
for s in queue:
stamp_delta = Duration(nanoseconds=abs(s - stamp.nanoseconds))
if stamp_delta > self.slop:
continue # far over the slop
topic_stamps.append(
((Time(nanoseconds=s,
clock_type=stamp.clock_type)), stamp_delta))
if not topic_stamps:
self.lock.release()
return
topic_stamps = sorted(topic_stamps, key=lambda x: x[1])
stamps.append(topic_stamps)
for vv in itertools.product(*[list(zip(*s))[0] for s in stamps]):
vv = list(vv)
# insert the new message
if my_queue_index is not None:
vv.insert(my_queue_index, stamp)
qt = list(zip(self.queues, vv))
if (((max(vv) - min(vv)) < self.slop) and
(len([1 for q, t in qt if t.nanoseconds not in q]) == 0)):
msgs = [q[t.nanoseconds] for q, t in qt]
self.signalMessage(*msgs)
for q, t in qt:
del q[t.nanoseconds]
break # fast finish after the synchronization
self.lock.release()
def test_backwards_jump(self):
time_source = TimeSource(node=self.node)
clock = ROSClock()
time_source.attach_clock(clock)
time_source.ros_time_is_active = True
pre_cb = Mock()
post_cb = Mock()
threshold = JumpThreshold(
min_forward=None, min_backward=Duration(seconds=-0.5), on_clock_change=False)
handler = clock.create_jump_callback(
threshold, pre_callback=pre_cb, post_callback=post_cb)
self.publish_reversed_clock_messages()
pre_cb.assert_called()
post_cb.assert_called()
assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_NO_CHANGE
handler.unregister()
def test_time_source_attach_clock(self):
time_source = TimeSource(node=self.node)
# ROSClock is a specialization of Clock with ROS time methods.
time_source.attach_clock(ROSClock())
# Other clock types are not supported.
with self.assertRaises(ValueError):
time_source.attach_clock(Clock(clock_type=ClockType.SYSTEM_TIME))
with self.assertRaises(ValueError):
time_source.attach_clock(Clock(clock_type=ClockType.STEADY_TIME))
def test_with_jump_handle():
clock = ROSClock()
clock._set_ros_time_is_active(False)
post_callback = Mock()
threshold = JumpThreshold(min_forward=None,
min_backward=None,
on_clock_change=True)
with clock.create_jump_callback(
threshold, post_callback=post_callback) as jump_handler:
assert isinstance(jump_handler, JumpHandle)
clock._set_ros_time_is_active(True)
post_callback.assert_called_once()
post_callback.reset_mock()
clock._set_ros_time_is_active(False)
post_callback.assert_not_called()
def execution(self):
# Set calibration state to success
req = SetParameters.Request()
param = Parameter()
param.name = "calibration_state"
param.value.type = ParameterType.PARAMETER_INTEGER
param.value.integer_value = 4
req.parameters.append(param)
future = self.client_param.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def __init__(self,
node_name,
*,
cli_args=None,
namespace=None,
use_global_arguments=True,
start_parameter_services=True):
self._handle = None
self._parameters = {}
self.publishers = []
self.subscriptions = []
self.clients = []
self.services = []
self.timers = []
self.guards = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
namespace = namespace or ''
if not ok():
raise NotInitializedException('cannot create node')
try:
self._handle = _rclpy.rclpy_create_node(node_name, namespace,
cli_args,
use_global_arguments)
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
self._logger = get_logger(
_rclpy.rclpy_get_node_logger_name(self.handle))
# Clock that has support for ROS time.
# TODO(dhood): use sim time if parameter has been set on the node.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
if start_parameter_services:
self._parameter_service = ParameterService(self)
def test_clock_construction(self):
clock = Clock()
with self.assertRaises(TypeError):
clock = Clock(clock_type='STEADY_TIME')
clock = Clock(clock_type=ClockType.STEADY_TIME)
assert clock.clock_type == ClockType.STEADY_TIME
clock = Clock(clock_type=ClockType.SYSTEM_TIME)
assert clock.clock_type == ClockType.SYSTEM_TIME
# A subclass ROSClock is returned if ROS_TIME is specified.
clock = Clock(clock_type=ClockType.ROS_TIME)
assert clock.clock_type == ClockType.ROS_TIME
assert isinstance(clock, ROSClock)
# Direct instantiation of a ROSClock is also possible.
clock = ROSClock()
assert clock.clock_type == ClockType.ROS_TIME
def execution(self):
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "robot2vision:\n"
output_string += " from: {}\n".format(from_frame)
output_string += " to: {}\n".format(to_frame)
output_string += " transform:\n"
output_string += " translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " rotation: [{}, {}, {}, {}]\n".format(
q[1], q[2], q[3], q[0])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def _publish_status(self, info_str=''):
"""Publish current state of this container."""
# Construct messages
with self._status_pub_lock:
path = self._path
#print str(structure_msg)
# Construct status message
#print self._container.get_active_states()
state_msg = SmachContainerStatus(
header=Header(stamp=ROSClock().now().to_msg()),
path=path,
initial_states=self._container.get_initial_states(),
active_states=self._container.get_active_states(),
local_data=bytearray(
pickle.dumps(self._container.userdata._data, 2)),
info=info_str)
# Publish message
self._status_pub.publish(state_msg)
def _to_time_inst(msg, rostype, inst=None):
# Create an instance if we haven't been provided with one
if rostype == "time" and msg == "now":
return ROSClock().now().to_msg()
if inst is None:
if rostype == "time":
inst = rclpy.time.Time().to_msg()
elif rostype == "duration":
inst = rclpy.duration.Duration().to_msg()
else:
return None
# Copy across the fields
for field in ["secs", "nsecs"]:
try:
if field in msg:
setattr(inst, field, msg[field])
except TypeError:
continue
return inst
def __init__(
self,
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,
allow_undeclared_parameters: bool = False,
automatically_declare_initial_parameters: bool = True) -> None:
"""
Constructor.
:param node_name: A name to give to this node. Validated by :func:`validate_node_name`.
: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.
Validated by :func:`validate_namespace`.
: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 parameters to be set during node creation.
:param allow_undeclared_parameters: True if undeclared parameters are allowed.
This flag affects the behavior of parameter-related operations.
:param automatically_declare_initial_parameters: True if initial parameters have to be
declared upon node creation, false otherwise.
"""
self.__handle = None
self._context = get_default_context() if context is None else context
self._parameters: dict = {}
self.__publishers: List[Publisher] = []
self.__subscriptions: List[Subscription] = []
self.__clients: List[Client] = []
self.__services: List[Service] = []
self.__timers: List[WallTimer] = []
self.__guards: List[GuardCondition] = []
self.__waitables: List[Waitable] = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
self._allow_undeclared_parameters = allow_undeclared_parameters
self._initial_parameters = {}
self._descriptors = {}
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self.__handle = Handle(
_rclpy.rclpy_create_node(node_name, namespace,
self._context.handle, cli_args,
use_global_arguments))
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
with self.handle as capsule:
self._logger = get_logger(
_rclpy.rclpy_get_node_logger_name(capsule))
# Clock that has support for ROS time.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent,
'parameter_events',
qos_profile=qos_profile_parameter_events)
with self.handle as capsule:
self._initial_parameters = _rclpy.rclpy_get_node_parameters(
Parameter, capsule)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
self._initial_parameters.update(
{p.name: p
for p in initial_parameters})
if automatically_declare_initial_parameters:
self._parameters.update(self._initial_parameters)
self._descriptors.update(
{p: ParameterDescriptor()
for p in self._parameters})
if start_parameter_services:
self._parameter_service = ParameterService(self)
def test_approx(self):
# Simple case, pairs of messages,
# make sure that they get combined
m0 = MockFilter()
m1 = MockFilter()
ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.1)
ts.registerCallback(self.cb_collector_2msg)
for t in range(10):
self.collector = []
msg0 = MockMessage(Time(seconds=t), 33)
msg1 = MockMessage(Time(seconds=t), 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N.
# Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(Time(seconds=t), random.random())\
for t in range(N)]
seq1 = [MockMessage(Time(seconds=t), random.random())\
for t in range(N)]
# random.shuffle(seq0)
ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
# test headerless scenarios: scramble sequences of length N of
# headerless and header-having messages.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [
MockMessage((ROSClock().now() + Duration(seconds=t)),
random.random()) for t in range(N)
]
seq1 = [MockHeaderlessMessage(random.random()) for t in range(N)]
ts = ApproximateTimeSynchronizer([m0, m1],
N,
10,
allow_headerless=True)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for i in range(N):
msg = seq1[i]
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
class Node:
"""
A Node in the ROS graph.
A Node is the primary entrypoint in a ROS system for communication.
It can be used to create ROS entities such as publishers, subscribers, services, etc.
"""
def __init__(self,
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) -> None:
"""
Constructor.
:param node_name: A name to give to this node. Validated by :func:`validate_node_name`.
: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.
Validated by :func:`validate_namespace`.
: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 parameters to be set during node creation.
"""
self.__handle = None
self._context = get_default_context() if context is None else context
self._parameters: dict = {}
self.__publishers: List[Publisher] = []
self.__subscriptions: List[Subscription] = []
self.__clients: List[Client] = []
self.__services: List[Service] = []
self.__timers: List[WallTimer] = []
self.__guards: List[GuardCondition] = []
self.__waitables: List[Waitable] = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self.__handle = Handle(
_rclpy.rclpy_create_node(node_name, namespace,
self._context.handle, cli_args,
use_global_arguments))
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
with self.handle as capsule:
self._logger = get_logger(
_rclpy.rclpy_get_node_logger_name(capsule))
# Clock that has support for ROS time.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent,
'parameter_events',
qos_profile=qos_profile_parameter_events)
with self.handle as capsule:
node_parameters = _rclpy.rclpy_get_node_parameters(
Parameter, capsule)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
node_parameters.update({p.name: p for p in initial_parameters})
self.set_parameters_atomically(node_parameters.values())
if start_parameter_services:
self._parameter_service = ParameterService(self)
@property
def publishers(self) -> Iterator[Publisher]:
"""Get publishers that have been created on this node."""
yield from self.__publishers
@property
def subscriptions(self) -> Iterator[Subscription]:
"""Get subscriptions that have been created on this node."""
yield from self.__subscriptions
@property
def clients(self) -> Iterator[Client]:
"""Get clients that have been created on this node."""
yield from self.__clients
@property
def services(self) -> Iterator[Service]:
"""Get services that have been created on this node."""
yield from self.__services
@property
def timers(self) -> Iterator[WallTimer]:
"""Get timers that have been created on this node."""
yield from self.__timers
@property
def guards(self) -> Iterator[GuardCondition]:
"""Get guards that have been created on this node."""
yield from self.__guards
@property
def waitables(self) -> Iterator[Waitable]:
"""Get waitables that have been created on this node."""
yield from self.__waitables
@property
def executor(self) -> Optional[Executor]:
"""Get the executor if the node has been added to one, else return ``None``."""
if self.__executor_weakref:
return self.__executor_weakref()
return None
@executor.setter
def executor(self, new_executor: Executor) -> None:
"""Set or change the executor the node belongs to."""
current_executor = self.executor
if current_executor == new_executor:
return
if current_executor is not None:
current_executor.remove_node(self)
if new_executor is None:
self.__executor_weakref = None
else:
new_executor.add_node(self)
self.__executor_weakref = weakref.ref(new_executor)
@property
def context(self) -> Context:
"""Get the context associated with the node."""
return self._context
@property
def default_callback_group(self) -> CallbackGroup:
"""
Get the default callback group.
If no other callback group is provided when the a ROS entity is created with the node,
then it is added to the default callback group.
"""
return self._default_callback_group
@property
def handle(self):
"""
Get the handle to the underlying `rcl_node_t`.
Cannot be modified after node creation.
:raises AttributeError: if modified after creation.
"""
return self.__handle
@handle.setter
def handle(self, value):
raise AttributeError('handle cannot be modified after node creation')
def get_name(self) -> str:
"""Get the name of the node."""
with self.handle as capsule:
return _rclpy.rclpy_get_node_name(capsule)
def get_namespace(self) -> str:
"""Get the namespace of the node."""
with self.handle as capsule:
return _rclpy.rclpy_get_node_namespace(capsule)
def get_clock(self) -> Clock:
"""Get the clock used by the node."""
return self._clock
def get_logger(self):
"""Get the nodes logger."""
return self._logger
def get_parameters(self, names: List[str]) -> List[Parameter]:
"""
Get a list of parameters.
:param names: The names of the parameters to get.
:return: The values for the given parameter names.
"""
if not all(isinstance(name, str) for name in names):
raise TypeError('All names must be instances of type str')
return [self.get_parameter(name) for name in names]
def get_parameter(self, name: str) -> Parameter:
"""
Get a parameter by name.
:param name: The name of the parameter.
:return: The value of the parameter.
"""
if name not in self._parameters:
return Parameter(name, Parameter.Type.NOT_SET, None)
return self._parameters[name]
def set_parameters(
self,
parameter_list: List[Parameter]) -> List[SetParametersResult]:
"""
Set parameters for the node.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node.
For each successfully set parameter, a :class:`ParameterEvent` message is
published.
:param parameter_list: The list of parameters to set.
:return: A list of SetParametersResult messages.
"""
results = []
for param in parameter_list:
if not isinstance(param, Parameter):
raise TypeError(
"parameter must be instance of type '{}'".format(
repr(Parameter)))
results.append(self.set_parameters_atomically([param]))
return results
def set_parameters_atomically(
self, parameter_list: List[Parameter]) -> SetParametersResult:
"""
Atomically set parameters for the node.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node.
If the parameters are set successfully, a :class:`ParameterEvent` message is
published.
:param parameter_list: The list of parameters to set.
"""
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace(
) + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter(
param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter(
param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(
param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def set_parameters_callback(
self, callback: Callable[[List[Parameter]],
SetParametersResult]) -> None:
"""
Register a set parameters callback.
Calling this function with override any previously registered callback.
:param callback: The function that is called whenever parameters are set for the node.
"""
self._parameters_callback = callback
def _validate_topic_or_service_name(self,
topic_or_service_name,
*,
is_service=False):
name = self.get_name()
namespace = self.get_namespace()
validate_node_name(name)
validate_namespace(namespace)
validate_topic_name(topic_or_service_name, is_service=is_service)
expanded_topic_or_service_name = expand_topic_name(
topic_or_service_name, name, namespace)
validate_full_topic_name(expanded_topic_or_service_name,
is_service=is_service)
def add_waitable(self, waitable: Waitable) -> None:
"""
Add a class that is capable of adding things to the wait set.
:param waitable: An instance of a waitable that the node will add to the waitset.
"""
self.__waitables.append(waitable)
def remove_waitable(self, waitable: Waitable) -> None:
"""
Remove a Waitable that was previously added to the node.
:param waitable: The Waitable to remove.
"""
self.__waitables.remove(waitable)
def create_publisher(
self,
msg_type,
topic: str,
*,
qos_profile: QoSProfile = qos_profile_default) -> Publisher:
"""
Create a new publisher.
:param msg_type: The type of ROS messages the publisher will publish.
:param topic: The name of the topic the publisher will publish to.
:param qos_profile: The quality of service profile to apply to the publisher.
:return: The new publisher.
"""
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
with self.handle as node_capsule:
publisher_capsule = _rclpy.rclpy_create_publisher(
node_capsule, msg_type, topic,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
publisher_handle = Handle(publisher_capsule)
publisher_handle.requires(self.handle)
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile)
self.__publishers.append(publisher)
return publisher
def create_subscription(self,
msg_type,
topic: str,
callback: Callable[[MsgType], None],
*,
qos_profile: QoSProfile = qos_profile_default,
callback_group: CallbackGroup = None,
raw: bool = False) -> Subscription:
"""
Create a new subscription.
:param msg_type: The type of ROS messages the subscription will subscribe to.
:param topic: The name of the topic the subscription will subscribe to.
:param callback: A user-defined callback function that is called when a message is
received by the subscription.
:param qos_profile: The quality of service profile to apply to the subscription.
:param callback_group: The callback group for the subscription. If ``None``, then the
nodes default callback group is used.
:param raw: If ``True``, then received messages will be stored in raw binary
representation.
"""
if callback_group is None:
callback_group = self.default_callback_group
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
with self.handle as capsule:
subscription_capsule = _rclpy.rclpy_create_subscription(
capsule, msg_type, topic, qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
subscription_handle = Handle(subscription_capsule)
subscription_handle.requires(self.handle)
subscription = Subscription(subscription_handle, msg_type, topic,
callback, callback_group, qos_profile, raw)
self.__subscriptions.append(subscription)
callback_group.add_entity(subscription)
return subscription
def create_client(self,
srv_type,
srv_name: str,
*,
qos_profile: QoSProfile = qos_profile_services_default,
callback_group: CallbackGroup = None) -> Client:
"""
Create a new service client.
:param srv_type: The service type.
:param srv_name: The name of the service.
:param qos_profile: The quality of service profile to apply the service client.
:param callback_group: The callback group for the service client. If ``None``, then the
nodes default callback group is used.
"""
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
with self.handle as node_capsule:
client_capsule = _rclpy.rclpy_create_client(
node_capsule, srv_type, srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
client_handle = Handle(client_capsule)
client_handle.requires(self.handle)
client = Client(self.context, client_handle, srv_type, srv_name,
qos_profile, callback_group)
self.__clients.append(client)
callback_group.add_entity(client)
return client
def create_service(self,
srv_type,
srv_name: str,
callback: Callable[[SrvTypeRequest, SrvTypeResponse],
SrvTypeResponse],
*,
qos_profile: QoSProfile = qos_profile_services_default,
callback_group: CallbackGroup = None) -> Service:
"""
Create a new service server.
:param srv_type: The service type.
:param srv_name: The name of the service.
:param callback: A user-defined callback function that is called when a service request
received by the server.
:param qos_profile: The quality of service profile to apply the service server.
:param callback_group: The callback group for the service server. If ``None``, then the
nodes default callback group is used.
"""
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
with self.handle as node_capsule:
service_capsule = _rclpy.rclpy_create_service(
node_capsule, srv_type, srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
service_handle = Handle(service_capsule)
service_handle.requires(self.handle)
service = Service(service_handle, srv_type, srv_name, callback,
callback_group, qos_profile)
self.__services.append(service)
callback_group.add_entity(service)
return service
def create_timer(self,
timer_period_sec: float,
callback: Callable,
callback_group: CallbackGroup = None) -> WallTimer:
"""
Create a new timer.
The timer will be started and every ``timer_period_sec`` number of seconds the provided
callback function will be called.
:param timer_period_sec: The period (s) of the timer.
:param callback: A user-defined callback function that is called when the timer expires.
:param callback_group: The callback group for the timer. If ``None``, then the nodes
default callback group is used.
"""
timer_period_nsec = int(float(timer_period_sec) * S_TO_NS)
if callback_group is None:
callback_group = self.default_callback_group
timer = WallTimer(callback,
callback_group,
timer_period_nsec,
context=self.context)
timer.handle.requires(self.handle)
self.__timers.append(timer)
callback_group.add_entity(timer)
return timer
def create_guard_condition(
self,
callback: Callable,
callback_group: CallbackGroup = None) -> GuardCondition:
"""Create a new guard condition."""
if callback_group is None:
callback_group = self.default_callback_group
guard = GuardCondition(callback, callback_group, context=self.context)
guard.handle.requires(self.handle)
self.__guards.append(guard)
callback_group.add_entity(guard)
return guard
def destroy_publisher(self, publisher: Publisher) -> bool:
"""
Destroy a publisher created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if publisher in self.__publishers:
self.__publishers.remove(publisher)
try:
publisher.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_subscription(self, subscription: Subscription) -> bool:
"""
Destroy a subscription created by the node.
:return: ``True`` if succesful, ``False`` otherwise.
"""
if subscription in self.__subscriptions:
self.__subscriptions.remove(subscription)
try:
subscription.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_client(self, client: Client) -> bool:
"""
Destroy a service client created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if client in self.__clients:
self.__clients.remove(client)
try:
client.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_service(self, service: Service) -> bool:
"""
Destroy a service server created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if service in self.__services:
self.__services.remove(service)
try:
service.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_timer(self, timer: WallTimer) -> bool:
"""
Destroy a timer created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if timer in self.__timers:
self.__timers.remove(timer)
try:
timer.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_guard_condition(self, guard: GuardCondition) -> bool:
"""
Destroy a guard condition created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if guard in self.__guards:
self.__guards.remove(guard)
try:
guard.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_node(self) -> bool:
"""
Destroy the node.
Frees resources used by the node, including any entities created by the following methods:
* :func:`create_publisher`
* :func:`create_subscription`
* :func:`create_client`
* :func:`create_service`
* :func:`create_timer`
* :func:`create_guard_condition`
"""
# Drop extra reference to parameter event publisher.
# It will be destroyed with other publishers below.
self._parameter_event_publisher = None
self.__publishers.clear()
self.__subscriptions.clear()
self.__clients.clear()
self.__services.clear()
self.__timers.clear()
self.__guards.clear()
self.handle.destroy()
def get_publisher_names_and_types_by_node(
self,
node_name: str,
node_namespace: str,
no_demangle: bool = False) -> List[Tuple[str, str]]:
"""
Get a list of discovered topics for publishers of a remote node.
:param node_name: Name of a remote node to get publishers for.
:param node_namespace: Namespace of the remote node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_publisher_names_and_types_by_node(
capsule, no_demangle, node_name, node_namespace)
def get_subscriber_names_and_types_by_node(
self,
node_name: str,
node_namespace: str,
no_demangle: bool = False) -> List[Tuple[str, str]]:
"""
Get a list of discovered topics for subscriptions of a remote node.
:param node_name: Name of a remote node to get subscriptions for.
:param node_namespace: Namespace of the remote node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_subscriber_names_and_types_by_node(
capsule, no_demangle, node_name, node_namespace)
def get_service_names_and_types_by_node(
self, node_name: str,
node_namespace: str) -> List[Tuple[str, str]]:
"""
Get a list of discovered service topics for a remote node.
:param node_name: Name of a remote node to get services for.
:param node_namespace: Namespace of the remote node.
:return: List of tuples containing two strings: the service name and service type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_service_names_and_types_by_node(
capsule, node_name, node_namespace)
def get_topic_names_and_types(self,
no_demangle: bool = False
) -> List[Tuple[str, str]]:
"""
Get a list topic names and types for the node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle)
def get_service_names_and_types(self) -> List[Tuple[str, str]]:
"""
Get a list of service topics for the node.
:return: List of tuples containing two strings: the service name and service type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_service_names_and_types(capsule)
def get_node_names(self) -> List[str]:
"""
Get a list of names for discovered nodes.
:return: List of node names.
"""
with self.handle as capsule:
names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule)
return [n[0] for n in names_ns]
def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]:
"""
Get a list of names and namespaces for discovered nodes.
:return: List of tuples containing two strings: the node name and node namespace.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_node_names_and_namespaces(capsule)
def _count_publishers_or_subscribers(self, topic_name, func):
fq_topic_name = expand_topic_name(topic_name, self.get_name(),
self.get_namespace())
validate_topic_name(fq_topic_name)
with self.handle as node_capsule:
return func(node_capsule, fq_topic_name)
def count_publishers(self, topic_name: str) -> int:
"""
Return the number of publishers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of publishers.
:return: the number of publishers on the topic.
"""
return self._count_publishers_or_subscribers(
topic_name, _rclpy.rclpy_count_publishers)
def count_subscribers(self, topic_name: str) -> int:
"""
Return the number of subscribers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of subscribers.
:return: the number of subscribers on the topic.
"""
return self._count_publishers_or_subscribers(
topic_name, _rclpy.rclpy_count_subscribers)
class Node:
def __init__(
self, node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True,
start_parameter_services=True, initial_parameters=None
):
self._handle = None
self._context = get_default_context() if context is None else context
self._parameters = {}
self.publishers = []
self.subscriptions = []
self.clients = []
self.services = []
self.timers = []
self.guards = []
self.waitables = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self._handle = _rclpy.rclpy_create_node(
node_name, namespace, self._context.handle, cli_args, use_global_arguments)
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle))
# Clock that has support for ROS time.
# TODO(dhood): use sim time if parameter has been set on the node.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events)
node_parameters = _rclpy.rclpy_get_node_parameters(Parameter, self.handle)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
node_parameters.update({p.name: p for p in initial_parameters})
self.set_parameters_atomically(node_parameters.values())
if start_parameter_services:
self._parameter_service = ParameterService(self)
@property
def executor(self):
"""Get the executor if the node has been added to one, else return None."""
if self.__executor_weakref:
return self.__executor_weakref()
@executor.setter
def executor(self, new_executor):
"""Set or change the executor the node belongs to."""
current_executor = self.executor
if current_executor is not None:
current_executor.remove_node(self)
if new_executor is None:
self.__executor_weakref = None
elif new_executor.add_node(self):
self.__executor_weakref = weakref.ref(new_executor)
@property
def context(self):
return self._context
@property
def default_callback_group(self):
return self._default_callback_group
@property
def handle(self):
return self._handle
@handle.setter
def handle(self, value):
raise AttributeError('handle cannot be modified after node creation')
def get_name(self):
return _rclpy.rclpy_get_node_name(self.handle)
def get_namespace(self):
return _rclpy.rclpy_get_node_namespace(self.handle)
def get_clock(self):
return self._clock
def get_logger(self):
return self._logger
def get_parameters(self, names):
if not all(isinstance(name, str) for name in names):
raise TypeError('All names must be instances of type str')
return [self.get_parameter(name) for name in names]
def get_parameter(self, name):
if name not in self._parameters:
return Parameter(name, Parameter.Type.NOT_SET, None)
return self._parameters[name]
def set_parameters(self, parameter_list):
results = []
for param in parameter_list:
if not isinstance(param, Parameter):
raise TypeError("parameter must be instance of type '{}'".format(repr(Parameter)))
results.append(self.set_parameters_atomically([param]))
return results
def set_parameters_atomically(self, parameter_list):
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace() + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter(param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter(param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def set_parameters_callback(self, callback):
self._parameters_callback = callback
def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False):
name = self.get_name()
namespace = self.get_namespace()
validate_node_name(name)
validate_namespace(namespace)
validate_topic_name(topic_or_service_name, is_service=is_service)
expanded_topic_or_service_name = expand_topic_name(topic_or_service_name, name, namespace)
validate_full_topic_name(expanded_topic_or_service_name, is_service=is_service)
def add_waitable(self, waitable):
"""Add a class which itself is capable of add things to the wait set."""
self.waitables.append(waitable)
def remove_waitable(self, waitable):
"""Remove a class which itself is capable of add things to the wait set."""
self.waitables.remove(waitable)
def create_publisher(self, msg_type, topic, *, qos_profile=qos_profile_default):
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
publisher_handle = _rclpy.rclpy_create_publisher(
self.handle, msg_type, topic, qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle)
self.publishers.append(publisher)
return publisher
def create_subscription(
self, msg_type, topic, callback, *, qos_profile=qos_profile_default,
callback_group=None, raw=False):
if callback_group is None:
callback_group = self.default_callback_group
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
[subscription_handle, subscription_pointer] = _rclpy.rclpy_create_subscription(
self.handle, msg_type, topic, qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
subscription = Subscription(
subscription_handle, subscription_pointer, msg_type,
topic, callback, callback_group, qos_profile, self.handle, raw)
self.subscriptions.append(subscription)
callback_group.add_entity(subscription)
return subscription
def create_client(
self, srv_type, srv_name, *, qos_profile=qos_profile_services_default,
callback_group=None):
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
[client_handle, client_pointer] = _rclpy.rclpy_create_client(
self.handle,
srv_type,
srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
client = Client(
self.handle, self.context,
client_handle, client_pointer, srv_type, srv_name, qos_profile,
callback_group)
self.clients.append(client)
callback_group.add_entity(client)
return client
def create_service(
self, srv_type, srv_name, callback, *, qos_profile=qos_profile_services_default,
callback_group=None):
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
[service_handle, service_pointer] = _rclpy.rclpy_create_service(
self.handle,
srv_type,
srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
service = Service(
self.handle, service_handle, service_pointer,
srv_type, srv_name, callback, callback_group, qos_profile)
self.services.append(service)
callback_group.add_entity(service)
return service
def create_timer(self, timer_period_sec, callback, callback_group=None):
timer_period_nsec = int(float(timer_period_sec) * S_TO_NS)
if callback_group is None:
callback_group = self.default_callback_group
timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context)
self.timers.append(timer)
callback_group.add_entity(timer)
return timer
def create_guard_condition(self, callback, callback_group=None):
if callback_group is None:
callback_group = self.default_callback_group
guard = GuardCondition(callback, callback_group, context=self.context)
self.guards.append(guard)
callback_group.add_entity(guard)
return guard
def destroy_publisher(self, publisher):
for pub in self.publishers:
if pub.publisher_handle == publisher.publisher_handle:
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
self.publishers.remove(pub)
return True
return False
def destroy_subscription(self, subscription):
for sub in self.subscriptions:
if sub.subscription_handle == subscription.subscription_handle:
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle)
self.subscriptions.remove(sub)
return True
return False
def destroy_client(self, client):
for cli in self.clients:
if cli.client_handle == client.client_handle:
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
self.clients.remove(cli)
return True
return False
def destroy_service(self, service):
for srv in self.services:
if srv.service_handle == service.service_handle:
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
self.services.remove(srv)
return True
return False
def destroy_timer(self, timer):
for tmr in self.timers:
if tmr.timer_handle == timer.timer_handle:
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
self.timers.remove(tmr)
return True
return False
def destroy_guard_condition(self, guard):
for gc in self.guards:
if gc.guard_handle == guard.guard_handle:
_rclpy.rclpy_destroy_entity(gc.guard_handle)
self.guards.remove(gc)
return True
return False
def destroy_node(self):
ret = True
if self.handle is None:
return ret
# Drop extra reference to parameter event publisher.
# It will be destroyed with other publishers below.
self._parameter_event_publisher = None
while self.publishers:
pub = self.publishers.pop()
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
while self.subscriptions:
sub = self.subscriptions.pop()
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle)
while self.clients:
cli = self.clients.pop()
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
while self.services:
srv = self.services.pop()
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
while self.timers:
tmr = self.timers.pop()
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
while self.guards:
gc = self.guards.pop()
_rclpy.rclpy_destroy_entity(gc.guard_handle)
_rclpy.rclpy_destroy_entity(self.handle)
self._handle = None
return ret
def get_publisher_names_and_types_by_node(self, node_name, node_namespace, no_demangle=False):
return _rclpy.rclpy_get_publisher_names_and_types_by_node(
self.handle, no_demangle, node_name, node_namespace)
def get_subscriber_names_and_types_by_node(self, node_name, node_namespace, no_demangle=False):
return _rclpy.rclpy_get_subscriber_names_and_types_by_node(
self.handle, no_demangle, node_name, node_namespace)
def get_service_names_and_types_by_node(self, node_name, node_namespace):
return _rclpy.rclpy_get_service_names_and_types_by_node(
self.handle, node_name, node_namespace)
def get_topic_names_and_types(self, no_demangle=False):
return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle)
def get_service_names_and_types(self):
return _rclpy.rclpy_get_service_names_and_types(self.handle)
def get_node_names(self):
names_ns = _rclpy.rclpy_get_node_names_and_namespaces(self.handle)
return [n[0] for n in names_ns]
def get_node_names_and_namespaces(self):
return _rclpy.rclpy_get_node_names_and_namespaces(self.handle)
def _count_publishers_or_subscribers(self, topic_name, func):
fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace())
validate_topic_name(fq_topic_name)
return func(self.handle, fq_topic_name)
def count_publishers(self, topic_name):
"""
Return the number of publishers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of publishers.
:type topic_name: str
:return: the number of publishers on the topic.
"""
return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers)
def count_subscribers(self, topic_name):
"""
Return the number of subscribers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of subscribers.
:type topic_name: str
:return: the number of subscribers on the topic.
"""
return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers)
def __del__(self):
self.destroy_node()
class Node:
"""
A Node in the ROS graph.
A Node is the primary entrypoint in a ROS system for communication.
It can be used to create ROS entities such as publishers, subscribers, services, etc.
"""
def __init__(
self,
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,
allow_undeclared_parameters: bool = False,
automatically_declare_initial_parameters: bool = True) -> None:
"""
Constructor.
:param node_name: A name to give to this node. Validated by :func:`validate_node_name`.
: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.
Validated by :func:`validate_namespace`.
: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 parameters to be set during node creation.
:param allow_undeclared_parameters: True if undeclared parameters are allowed.
This flag affects the behavior of parameter-related operations.
:param automatically_declare_initial_parameters: True if initial parameters have to be
declared upon node creation, false otherwise.
"""
self.__handle = None
self._context = get_default_context() if context is None else context
self._parameters: dict = {}
self.__publishers: List[Publisher] = []
self.__subscriptions: List[Subscription] = []
self.__clients: List[Client] = []
self.__services: List[Service] = []
self.__timers: List[WallTimer] = []
self.__guards: List[GuardCondition] = []
self.__waitables: List[Waitable] = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()
self._parameters_callback = None
self._allow_undeclared_parameters = allow_undeclared_parameters
self._initial_parameters = {}
self._descriptors = {}
namespace = namespace or ''
if not self._context.ok():
raise NotInitializedException('cannot create node')
try:
self.__handle = Handle(
_rclpy.rclpy_create_node(node_name, namespace,
self._context.handle, cli_args,
use_global_arguments))
except ValueError:
# these will raise more specific errors if the name or namespace is bad
validate_node_name(node_name)
# emulate what rcl_node_init() does to accept '' and relative namespaces
if not namespace:
namespace = '/'
if not namespace.startswith('/'):
namespace = '/' + namespace
validate_namespace(namespace)
# Should not get to this point
raise RuntimeError('rclpy_create_node failed for unknown reason')
with self.handle as capsule:
self._logger = get_logger(
_rclpy.rclpy_get_node_logger_name(capsule))
# Clock that has support for ROS time.
self._clock = ROSClock()
self._time_source = TimeSource(node=self)
self._time_source.attach_clock(self._clock)
self.__executor_weakref = None
self._parameter_event_publisher = self.create_publisher(
ParameterEvent,
'parameter_events',
qos_profile=qos_profile_parameter_events)
with self.handle as capsule:
self._initial_parameters = _rclpy.rclpy_get_node_parameters(
Parameter, capsule)
# Combine parameters from params files with those from the node constructor and
# use the set_parameters_atomically API so a parameter event is published.
if initial_parameters is not None:
self._initial_parameters.update(
{p.name: p
for p in initial_parameters})
if automatically_declare_initial_parameters:
self._parameters.update(self._initial_parameters)
self._descriptors.update(
{p: ParameterDescriptor()
for p in self._parameters})
if start_parameter_services:
self._parameter_service = ParameterService(self)
@property
def publishers(self) -> Iterator[Publisher]:
"""Get publishers that have been created on this node."""
yield from self.__publishers
@property
def subscriptions(self) -> Iterator[Subscription]:
"""Get subscriptions that have been created on this node."""
yield from self.__subscriptions
@property
def clients(self) -> Iterator[Client]:
"""Get clients that have been created on this node."""
yield from self.__clients
@property
def services(self) -> Iterator[Service]:
"""Get services that have been created on this node."""
yield from self.__services
@property
def timers(self) -> Iterator[WallTimer]:
"""Get timers that have been created on this node."""
yield from self.__timers
@property
def guards(self) -> Iterator[GuardCondition]:
"""Get guards that have been created on this node."""
yield from self.__guards
@property
def waitables(self) -> Iterator[Waitable]:
"""Get waitables that have been created on this node."""
yield from self.__waitables
@property
def executor(self) -> Optional[Executor]:
"""Get the executor if the node has been added to one, else return ``None``."""
if self.__executor_weakref:
return self.__executor_weakref()
return None
@executor.setter
def executor(self, new_executor: Executor) -> None:
"""Set or change the executor the node belongs to."""
current_executor = self.executor
if current_executor == new_executor:
return
if current_executor is not None:
current_executor.remove_node(self)
if new_executor is None:
self.__executor_weakref = None
else:
new_executor.add_node(self)
self.__executor_weakref = weakref.ref(new_executor)
@property
def context(self) -> Context:
"""Get the context associated with the node."""
return self._context
@property
def default_callback_group(self) -> CallbackGroup:
"""
Get the default callback group.
If no other callback group is provided when the a ROS entity is created with the node,
then it is added to the default callback group.
"""
return self._default_callback_group
@property
def handle(self):
"""
Get the handle to the underlying `rcl_node_t`.
Cannot be modified after node creation.
:raises: AttributeError if modified after creation.
"""
return self.__handle
@handle.setter
def handle(self, value):
raise AttributeError('handle cannot be modified after node creation')
def get_name(self) -> str:
"""Get the name of the node."""
with self.handle as capsule:
return _rclpy.rclpy_get_node_name(capsule)
def get_namespace(self) -> str:
"""Get the namespace of the node."""
with self.handle as capsule:
return _rclpy.rclpy_get_node_namespace(capsule)
def get_clock(self) -> Clock:
"""Get the clock used by the node."""
return self._clock
def get_logger(self):
"""Get the nodes logger."""
return self._logger
def declare_parameter(
self,
name: str,
value: ParameterValue = ParameterValue(),
descriptor: ParameterDescriptor = ParameterDescriptor()
) -> Parameter:
"""
Declare and initialize a parameter.
This method, if successful, will result in any callback registered with
:func:`set_parameters_callback` to be called.
:param name: Fully-qualified name of the parameter, including its namespace.
:param value: Value of the parameter to declare.
:param descriptor: Descriptor for the parameter to declare.
:return: Parameter with the effectively assigned value.
:raises: ParameterAlreadyDeclaredException if the parameter had already been declared.
:raises: InvalidParameterException if the parameter name is invalid.
:raises: InvalidParameterValueException if the registered callback rejects the parameter.
"""
return self.declare_parameters('', [(name, value, descriptor)])[0]
def declare_parameters(
self, namespace: str,
parameters: List[Tuple[str, Optional[ParameterValue],
Optional[ParameterDescriptor]]]
) -> List[Parameter]:
"""
Declare a list of parameters.
This method, if successful, will result in any callback registered with
:func:`set_parameters_callback` to be called once for each parameter.
If one of those calls fail, an exception will be raised and the remaining parameters will
not be declared.
Parameters declared up to that point will not be undeclared.
:param namespace: Namespace for parameters.
:param parameters: Tuple with parameters to declare, with a name, value and descriptor.
:return: Parameter list with the effectively assigned values for each of them.
:raises: ParameterAlreadyDeclaredException if the parameter had already been declared.
:raises: InvalidParameterException if the parameter name is invalid.
:raises: InvalidParameterValueException if the registered callback rejects any parameter.
"""
parameter_list = []
descriptor_list = []
for parameter_tuple in parameters:
name = parameter_tuple[0]
assert isinstance(name, str)
# Get value from initial parameters, of from tuple if it doesn't exist.
if name in self._initial_parameters:
value = self._initial_parameters[name].get_parameter_value()
elif parameter_tuple[1] is None:
value = ParameterValue()
else:
value = parameter_tuple[1]
assert isinstance(value, ParameterValue)
descriptor = parameter_tuple[2]
if descriptor is None:
descriptor = ParameterDescriptor()
assert isinstance(descriptor, ParameterDescriptor)
# Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't.
full_name = namespace + name
validate_parameter_name(full_name)
parameter_list.append(
Parameter.from_parameter_msg(
ParameterMsg(name=full_name, value=value)))
descriptor_list.append(descriptor)
parameters_already_declared = [
parameter.name for parameter in parameter_list
if parameter.name in self._parameters
]
if any(parameters_already_declared):
raise ParameterAlreadyDeclaredException(
parameters_already_declared)
# Call the callback once for each of the parameters, using method that doesn't
# check whether the parameter was declared beforehand or not.
self._set_parameters(parameter_list,
descriptor_list=descriptor_list,
raise_on_failure=True)
return self.get_parameters(
[parameter.name for parameter in parameter_list])
def undeclare_parameter(self, name: str):
"""
Undeclare a previously declared parameter.
This method will not cause a callback registered with
:func:`set_parameters_callback` to be called.
:param name: Fully-qualified name of the parameter, including its namespace.
:raises: ParameterNotDeclaredException if parameter had not been declared before.
:raises: ParameterImmutableException if the parameter was created as read-only.
"""
if self.has_parameter(name):
if self._descriptors[name].read_only:
raise ParameterImmutableException(name)
else:
del self._parameters[name]
del self._descriptors[name]
else:
raise ParameterNotDeclaredException(name)
def has_parameter(self, name: str) -> bool:
"""Return True if parameter is declared; False otherwise."""
return name in self._parameters
def get_parameters(self, names: List[str]) -> List[Parameter]:
"""
Get a list of parameters.
:param names: Fully-qualified names of the parameters to get, including their namespaces.
:return: The values for the given parameter names.
A default Parameter will be returned for undeclared parameters if
undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if undeclared parameters are not allowed,
and at least one parameter hadn't been declared beforehand.
"""
if not all(isinstance(name, str) for name in names):
raise TypeError('All names must be instances of type str')
return [self.get_parameter(name) for name in names]
def get_parameter(self, name: str) -> Parameter:
"""
Get a parameter by name.
:param name: Fully-qualified name of the parameter, including its namespace.
:return: The values for the given parameter names.
A default Parameter will be returned for an undeclared parameter if
undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if undeclared parameters are not allowed,
and the parameter hadn't been declared beforehand.
"""
if self.has_parameter(name):
return self._parameters[name]
elif self._allow_undeclared_parameters:
# If undeclared parameters are allowed, the parameter might be in the initial set.
# If that's the case, first set and then return.
if name in self._initial_parameters:
self._parameters.update({name: self._initial_parameters[name]})
self._descriptors.update({name: ParameterDescriptor()})
return self._parameters[name]
return Parameter(name, Parameter.Type.NOT_SET, None)
else:
raise ParameterNotDeclaredException(name)
def get_parameter_or(
self,
name: str,
alternative_value: Optional[Parameter] = None) -> Parameter:
"""
Get a parameter or the alternative value.
If the alternative value is None, a default Parameter with the given name and NOT_SET
type will be returned.
This method does not declare parameters in any case.
:param name: Fully-qualified name of the parameter, including its namespace.
:param alternative_value: Alternative parameter to get if it had not been declared before.
:return: Requested parameter, or alternative value if it hadn't been declared before.
"""
if alternative_value is None:
alternative_value = Parameter(name, Parameter.Type.NOT_SET)
return self._parameters.get(name, alternative_value)
def set_parameters(
self,
parameter_list: List[Parameter]) -> List[SetParametersResult]:
"""
Set parameters for the node, and return the result for the set action.
If any parameter in the list was not declared beforehand and undeclared parameters are not
allowed for the node, this method will raise a ParameterNotDeclaredException exception.
Parameters are set in the order they are declared in the list.
If setting a parameter fails due to not being declared, then the
parameters which have already been set will stay set, and no attempt will
be made to set the parameters which come after.
If undeclared parameters are allowed, then all the parameters will be implicitly
declared before being set even if they were not declared beforehand.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node, once for each parameter.
If the callback prevents a parameter from being set, then it will be reflected in the
returned result; no exceptions will be raised in this case.
For each successfully set parameter, a :class:`ParameterEvent` message is
published.
If the value type of the parameter is NOT_SET, and the existing parameter type is
something else, then the parameter will be implicitly undeclared.
:param parameter_list: The list of parameters to set.
:return: The result for each set action as a list.
:raises: ParameterNotDeclaredException if undeclared parameters are not allowed,
and at least one parameter in the list hadn't been declared beforehand.
"""
self._check_undeclared_parameters(parameter_list)
return self._set_parameters(parameter_list)
def _set_parameters(self,
parameter_list: List[Parameter],
descriptor_list: Optional[
List[ParameterDescriptor]] = None,
raise_on_failure=False) -> List[SetParametersResult]:
"""
Set parameters for the node, and return the result for the set action.
Method for internal usage; applies a setter method for each parameters in the list.
By default it doesn't check if the parameters were declared, and both declares and sets
the given list.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node, once for each parameter.
If the callback doesn't succeed for a given parameter, it won't be set and either an
unsuccessful result will be returned for that parameter, or an exception will be raised
according to `raise_on_failure` flag.
:param parameter_list: List of parameters to set.
:return: The result for each set action as a list.
:raises: InvalidParameterValueException if the user-defined callback rejects the
parameter value and raise_on_failure flag is True.
"""
if descriptor_list is not None:
assert len(descriptor_list) == len(parameter_list)
results = []
for index, param in enumerate(parameter_list):
result = self._set_parameters_atomically([param])
if raise_on_failure and not result.successful:
raise InvalidParameterValueException(param.name, param.value)
results.append(result)
if descriptor_list is not None:
self._descriptors[param.name] = descriptor_list[index]
return results
def set_parameters_atomically(
self, parameter_list: List[Parameter]) -> SetParametersResult:
"""
Set the given parameters, all at one time, and then aggregate result.
If any parameter in the list was not declared beforehand and undeclared parameters are not
allowed for the node, this method will raise a ParameterNotDeclaredException exception.
Parameters are set all at once.
If setting a parameter fails due to not being declared, then no parameter will be set set.
Either all of the parameters are set or none of them are set.
If undeclared parameters are allowed, then all the parameters will be implicitly
declared before being set even if they were not declared beforehand.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node only once for all parameters.
If the callback prevents the parameters from being set, then it will be reflected in the
returned result; no exceptions will be raised in this case.
For each successfully set parameter, a :class:`ParameterEvent` message is published.
If the value type of the parameter is NOT_SET, and the existing parameter type is
something else, then the parameter will be implicitly undeclared.
:param parameter_list: The list of parameters to set.
:return: Aggregate result of setting all the parameters atomically.
:raises: ParameterNotDeclaredException if undeclared parameters are not allowed,
and at least one parameter in the list hadn't been declared beforehand.
"""
self._check_undeclared_parameters(parameter_list)
return self._set_parameters_atomically(parameter_list)
def _check_undeclared_parameters(self, parameter_list: List[Parameter]):
"""
Check if parameter list has correct types and was declared beforehand.
:raises: ParameterNotDeclaredException if at least one parameter in the list was not
declared beforehand.
"""
if not all(
isinstance(parameter, Parameter)
for parameter in parameter_list):
raise TypeError("parameter must be instance of type '{}'".format(
repr(Parameter)))
undeclared_parameters = (param.name for param in parameter_list
if param.name not in self._parameters)
if (not self._allow_undeclared_parameters
and any(undeclared_parameters)):
raise ParameterNotDeclaredException(list(undeclared_parameters))
def _set_parameters_atomically(
self, parameter_list: List[Parameter]) -> SetParametersResult:
"""
Set the given parameters, all at one time, and then aggregate result.
This method does not check if the parameters were declared beforehand, and is intended
for internal use of this class.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node only once for all parameters.
If the callback prevents the parameters from being set, then it will be reflected in the
returned result; no exceptions will be raised in this case.
For each successfully set parameter, a :class:`ParameterEvent` message is
published.
If the value type of the parameter is NOT_SET, and the existing parameter type is
something else, then the parameter will be implicitly undeclared.
:param parameter_list: The list of parameters to set.
:return: Aggregate result of setting all the parameters atomically.
"""
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace(
) + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter_or(
param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
if param.name in self._descriptors:
del self._descriptors[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter_or(
param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(
param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def describe_parameter(self, name: str) -> ParameterDescriptor:
"""
Get the parameter descriptor of a given parameter.
:param name: Fully-qualified name of the parameter, including its namespace.
:return: ParameterDescriptor corresponding to the parameter,
or default ParameterDescriptor if parameter had not been declared before
and undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if parameter had not been declared before
and undeclared parameters are not allowed.
"""
try:
return self._descriptors[name]
except KeyError:
if self._allow_undeclared_parameters:
return ParameterDescriptor()
else:
raise ParameterNotDeclaredException(name)
def describe_parameters(self,
names: List[str]) -> List[ParameterDescriptor]:
"""
Get the parameter descriptors of a given list of parameters.
:param name: List of fully-qualified names of the parameters to describe.
:return: List of ParameterDescriptors corresponding to the given parameters.
Default ParameterDescriptors shall be returned for parameters that
had not been declared before if undeclared parameters are allowed.
:raises: ParameterNotDeclaredException if at least one parameter
had not been declared before and undeclared parameters are not allowed.
"""
parameter_descriptors = []
for name in names:
parameter_descriptors.append(self.describe_parameter(name))
return parameter_descriptors
def set_parameters_callback(
self, callback: Callable[[List[Parameter]],
SetParametersResult]) -> None:
"""
Register a set parameters callback.
Calling this function with override any previously registered callback.
:param callback: The function that is called whenever parameters are set for the node.
"""
self._parameters_callback = callback
def _validate_topic_or_service_name(self,
topic_or_service_name,
*,
is_service=False):
name = self.get_name()
namespace = self.get_namespace()
validate_node_name(name)
validate_namespace(namespace)
validate_topic_name(topic_or_service_name, is_service=is_service)
expanded_topic_or_service_name = expand_topic_name(
topic_or_service_name, name, namespace)
validate_full_topic_name(expanded_topic_or_service_name,
is_service=is_service)
def add_waitable(self, waitable: Waitable) -> None:
"""
Add a class that is capable of adding things to the wait set.
:param waitable: An instance of a waitable that the node will add to the waitset.
"""
self.__waitables.append(waitable)
def remove_waitable(self, waitable: Waitable) -> None:
"""
Remove a Waitable that was previously added to the node.
:param waitable: The Waitable to remove.
"""
self.__waitables.remove(waitable)
def create_publisher(
self,
msg_type,
topic: str,
*,
qos_profile: QoSProfile = qos_profile_default) -> Publisher:
"""
Create a new publisher.
:param msg_type: The type of ROS messages the publisher will publish.
:param topic: The name of the topic the publisher will publish to.
:param qos_profile: The quality of service profile to apply to the publisher.
:return: The new publisher.
"""
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
with self.handle as node_capsule:
publisher_capsule = _rclpy.rclpy_create_publisher(
node_capsule, msg_type, topic,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
publisher_handle = Handle(publisher_capsule)
publisher_handle.requires(self.handle)
publisher = Publisher(publisher_handle, msg_type, topic, qos_profile)
self.__publishers.append(publisher)
return publisher
def create_subscription(self,
msg_type,
topic: str,
callback: Callable[[MsgType], None],
*,
qos_profile: QoSProfile = qos_profile_default,
callback_group: CallbackGroup = None,
raw: bool = False) -> Subscription:
"""
Create a new subscription.
:param msg_type: The type of ROS messages the subscription will subscribe to.
:param topic: The name of the topic the subscription will subscribe to.
:param callback: A user-defined callback function that is called when a message is
received by the subscription.
:param qos_profile: The quality of service profile to apply to the subscription.
:param callback_group: The callback group for the subscription. If ``None``, then the
nodes default callback group is used.
:param raw: If ``True``, then received messages will be stored in raw binary
representation.
"""
if callback_group is None:
callback_group = self.default_callback_group
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
try:
with self.handle as capsule:
subscription_capsule = _rclpy.rclpy_create_subscription(
capsule, msg_type, topic, qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(topic)
subscription_handle = Handle(subscription_capsule)
subscription_handle.requires(self.handle)
subscription = Subscription(subscription_handle, msg_type, topic,
callback, callback_group, qos_profile, raw)
self.__subscriptions.append(subscription)
callback_group.add_entity(subscription)
return subscription
def create_client(self,
srv_type,
srv_name: str,
*,
qos_profile: QoSProfile = qos_profile_services_default,
callback_group: CallbackGroup = None) -> Client:
"""
Create a new service client.
:param srv_type: The service type.
:param srv_name: The name of the service.
:param qos_profile: The quality of service profile to apply the service client.
:param callback_group: The callback group for the service client. If ``None``, then the
nodes default callback group is used.
"""
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
with self.handle as node_capsule:
client_capsule = _rclpy.rclpy_create_client(
node_capsule, srv_type, srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
client_handle = Handle(client_capsule)
client_handle.requires(self.handle)
client = Client(self.context, client_handle, srv_type, srv_name,
qos_profile, callback_group)
self.__clients.append(client)
callback_group.add_entity(client)
return client
def create_service(self,
srv_type,
srv_name: str,
callback: Callable[[SrvTypeRequest, SrvTypeResponse],
SrvTypeResponse],
*,
qos_profile: QoSProfile = qos_profile_services_default,
callback_group: CallbackGroup = None) -> Service:
"""
Create a new service server.
:param srv_type: The service type.
:param srv_name: The name of the service.
:param callback: A user-defined callback function that is called when a service request
received by the server.
:param qos_profile: The quality of service profile to apply the service server.
:param callback_group: The callback group for the service server. If ``None``, then the
nodes default callback group is used.
"""
if callback_group is None:
callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
with self.handle as node_capsule:
service_capsule = _rclpy.rclpy_create_service(
node_capsule, srv_type, srv_name,
qos_profile.get_c_qos_profile())
except ValueError:
failed = True
if failed:
self._validate_topic_or_service_name(srv_name, is_service=True)
service_handle = Handle(service_capsule)
service_handle.requires(self.handle)
service = Service(service_handle, srv_type, srv_name, callback,
callback_group, qos_profile)
self.__services.append(service)
callback_group.add_entity(service)
return service
def create_timer(self,
timer_period_sec: float,
callback: Callable,
callback_group: CallbackGroup = None) -> WallTimer:
"""
Create a new timer.
The timer will be started and every ``timer_period_sec`` number of seconds the provided
callback function will be called.
:param timer_period_sec: The period (s) of the timer.
:param callback: A user-defined callback function that is called when the timer expires.
:param callback_group: The callback group for the timer. If ``None``, then the nodes
default callback group is used.
"""
timer_period_nsec = int(float(timer_period_sec) * S_TO_NS)
if callback_group is None:
callback_group = self.default_callback_group
timer = WallTimer(callback,
callback_group,
timer_period_nsec,
context=self.context)
timer.handle.requires(self.handle)
self.__timers.append(timer)
callback_group.add_entity(timer)
return timer
def create_guard_condition(
self,
callback: Callable,
callback_group: CallbackGroup = None) -> GuardCondition:
"""Create a new guard condition."""
if callback_group is None:
callback_group = self.default_callback_group
guard = GuardCondition(callback, callback_group, context=self.context)
guard.handle.requires(self.handle)
self.__guards.append(guard)
callback_group.add_entity(guard)
return guard
def destroy_publisher(self, publisher: Publisher) -> bool:
"""
Destroy a publisher created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if publisher in self.__publishers:
self.__publishers.remove(publisher)
try:
publisher.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_subscription(self, subscription: Subscription) -> bool:
"""
Destroy a subscription created by the node.
:return: ``True`` if succesful, ``False`` otherwise.
"""
if subscription in self.__subscriptions:
self.__subscriptions.remove(subscription)
try:
subscription.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_client(self, client: Client) -> bool:
"""
Destroy a service client created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if client in self.__clients:
self.__clients.remove(client)
try:
client.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_service(self, service: Service) -> bool:
"""
Destroy a service server created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if service in self.__services:
self.__services.remove(service)
try:
service.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_timer(self, timer: WallTimer) -> bool:
"""
Destroy a timer created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if timer in self.__timers:
self.__timers.remove(timer)
try:
timer.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_guard_condition(self, guard: GuardCondition) -> bool:
"""
Destroy a guard condition created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if guard in self.__guards:
self.__guards.remove(guard)
try:
guard.destroy()
except InvalidHandle:
return False
return True
return False
def destroy_node(self) -> bool:
"""
Destroy the node.
Frees resources used by the node, including any entities created by the following methods:
* :func:`create_publisher`
* :func:`create_subscription`
* :func:`create_client`
* :func:`create_service`
* :func:`create_timer`
* :func:`create_guard_condition`
"""
# Drop extra reference to parameter event publisher.
# It will be destroyed with other publishers below.
self._parameter_event_publisher = None
self.__publishers.clear()
self.__subscriptions.clear()
self.__clients.clear()
self.__services.clear()
self.__timers.clear()
self.__guards.clear()
self.handle.destroy()
def get_publisher_names_and_types_by_node(
self,
node_name: str,
node_namespace: str,
no_demangle: bool = False) -> List[Tuple[str, str]]:
"""
Get a list of discovered topics for publishers of a remote node.
:param node_name: Name of a remote node to get publishers for.
:param node_namespace: Namespace of the remote node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_publisher_names_and_types_by_node(
capsule, no_demangle, node_name, node_namespace)
def get_subscriber_names_and_types_by_node(
self,
node_name: str,
node_namespace: str,
no_demangle: bool = False) -> List[Tuple[str, str]]:
"""
Get a list of discovered topics for subscriptions of a remote node.
:param node_name: Name of a remote node to get subscriptions for.
:param node_namespace: Namespace of the remote node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_subscriber_names_and_types_by_node(
capsule, no_demangle, node_name, node_namespace)
def get_service_names_and_types_by_node(
self, node_name: str,
node_namespace: str) -> List[Tuple[str, str]]:
"""
Get a list of discovered service topics for a remote node.
:param node_name: Name of a remote node to get services for.
:param node_namespace: Namespace of the remote node.
:return: List of tuples containing two strings: the service name and service type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_service_names_and_types_by_node(
capsule, node_name, node_namespace)
def get_topic_names_and_types(self,
no_demangle: bool = False
) -> List[Tuple[str, str]]:
"""
Get a list topic names and types for the node.
:param no_demangle: If ``True``, then topic names and types returned will not be demangled.
:return: List of tuples containing two strings: the topic name and topic type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle)
def get_service_names_and_types(self) -> List[Tuple[str, str]]:
"""
Get a list of service topics for the node.
:return: List of tuples containing two strings: the service name and service type.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_service_names_and_types(capsule)
def get_node_names(self) -> List[str]:
"""
Get a list of names for discovered nodes.
:return: List of node names.
"""
with self.handle as capsule:
names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule)
return [n[0] for n in names_ns]
def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]:
"""
Get a list of names and namespaces for discovered nodes.
:return: List of tuples containing two strings: the node name and node namespace.
"""
with self.handle as capsule:
return _rclpy.rclpy_get_node_names_and_namespaces(capsule)
def _count_publishers_or_subscribers(self, topic_name, func):
fq_topic_name = expand_topic_name(topic_name, self.get_name(),
self.get_namespace())
validate_topic_name(fq_topic_name)
with self.handle as node_capsule:
return func(node_capsule, fq_topic_name)
def count_publishers(self, topic_name: str) -> int:
"""
Return the number of publishers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of publishers.
:return: the number of publishers on the topic.
"""
return self._count_publishers_or_subscribers(
topic_name, _rclpy.rclpy_count_publishers)
def count_subscribers(self, topic_name: str) -> int:
"""
Return the number of subscribers on a given topic.
`topic_name` may be a relative, private, or fully qualifed topic name.
A relative or private topic is expanded using this node's namespace and name.
The queried topic name is not remapped.
:param topic_name: the topic_name on which to count the number of subscribers.
:return: the number of subscribers on the topic.
"""
return self._count_publishers_or_subscribers(
topic_name, _rclpy.rclpy_count_subscribers)
def assert_liveliness(self) -> None:
"""
Manually assert that this Node is alive.
If the QoS Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the
application must call this at least as often as ``QoSProfile.liveliness_lease_duration``.
"""
_rclpy.rclpy_assert_liveliness(self.node_handle)
