def test_sleep_for_negative_duration(default_context, clock_type):
clock = Clock(clock_type=clock_type)
sleep_duration = Duration(seconds=-1)
start = clock.now()
assert clock.sleep_for(sleep_duration)
stop = clock.now()
assert stop - start < A_SMALL_AMOUNT_OF_TIME
def __init__(self, com_pin=board.D18, num_pixels=16):
super().__init__('neopixel_node')
self.pixels = neopixel.NeoPixel(com_pin,
num_pixels,
brightness=self.DEFAULT_BRIGHTNESS)
self._connected = False
self._enabled = False
self._update_light()
self.declare_parameter('heartbeat_period_s', 0.5)
self._timeout_s = self.get_parameter('heartbeat_period_s').value * 2.0
secs = int(self._timeout_s)
nsecs = int((self._timeout_s - secs) * 1e9)
self._timeout_duration = Duration(seconds=secs, nanoseconds=nsecs)
self.get_logger().info('Starting with timeout {}'.format(
self._timeout_s))
self._clock = Clock()
self._enable_sub = self.create_subscription(Bool, 'neopixel/enable',
self._enable_callback, 1)
self._heartbeat_sub = self.create_subscription(
Header, 'heartbeat', self._heartbeat_callback, 1)
self._timer = self.create_timer(self._timeout_s, self._timer_callback)
self._timer.cancel()
def test_sleep_for_basic(default_context, clock_type):
clock = Clock(clock_type=clock_type)
sleep_duration = Duration(seconds=0.1)
start = clock.now()
assert clock.sleep_for(sleep_duration)
stop = clock.now()
assert stop - start >= sleep_duration
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_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 clear(self):
"""Reset the statistics."""
with self.lock:
self.count = 0
clock = Clock(clock_type=ClockType.STEADY_TIME)
curtime = clock.now()
self.times = [curtime for i in range(self.params.window_size)]
self.seq_nums = [0 for i in range(self.params.window_size)]
self.hist_indx = 0
def timer_callback(self):
clock = Clock(clock_type=ClockType.ROS_TIME)
message
message = Clock().now().to_msg()
# msg.data = 'Hello World: %d' % self.i
self.publisher_.publish(message)
self.get_logger().info('Publishing Time for : "%s"' % message)
def wait_for(
condition: Callable[[], bool],
timeout: Duration,
sleep_time: float = 0.1,
):
clock = Clock(clock_type=ClockType.STEADY_TIME)
start = clock.now()
while not condition():
if clock.now() - start > timeout:
return False
time.sleep(sleep_time)
return True
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 __init__(self):
super().__init__('heartbeat_publisher')
self.declare_parameter('heartbeat_period_s', 0.5)
heartbeat_period_s = self.get_parameter('heartbeat_period_s').value
self.get_logger().info(
'Starting heartbeat with period {}'.format(heartbeat_period_s))
self._pub = self.create_publisher(Header, 'heartbeat', 1)
self._clock = Clock()
self._timer = self.create_timer(heartbeat_period_s,
self.timer_callback)
def unblock_by_timeout():
nonlocal send_goal_future, goal, event
clock = Clock()
start_time = clock.now()
timeout = Duration.from_msg(goal.timeout)
timeout_padding = Duration(seconds=self.timeout_padding)
while not send_goal_future.done() and not event.is_set():
if clock.now() > start_time + timeout + timeout_padding:
break
# TODO(vinnamkim): rclpy.Rate is not ready
# See https://github.com/ros2/rclpy/issues/186
#r = rospy.Rate(self.check_frequency)
sleep(1.0 / self.check_frequency)
event.set()
def test_log_arguments(self):
system_clock = Clock()
# Check half-specified filter not allowed if a required parameter is missing
with self.assertRaisesRegex(TypeError,
'required parameter .* not specified'):
rclpy.logging._root_logger.log(
'message',
LoggingSeverity.INFO,
throttle_time_source_type=system_clock,
)
# Check half-specified filter is allowed if an optional parameter is missing
rclpy.logging._root_logger.log(
'message',
LoggingSeverity.INFO,
throttle_duration_sec=0.1,
)
# Check unused kwarg is not allowed
with self.assertRaisesRegex(
TypeError, 'parameter .* is not one of the recognized'):
rclpy.logging._root_logger.log(
'message',
LoggingSeverity.INFO,
name='my_name',
skip_first=True,
unused_kwarg='unused_kwarg',
)
def callback_hz(self, m, topic=None):
"""
Calculate interval time.
:param m: Message instance
:param topic: Topic name
"""
# ignore messages that don't match filter
if self.filter_expr is not None and not self.filter_expr(m):
return
with self.lock:
# Uses ROS time as the default time source and Walltime only if requested
curr_rostime = self._clock.now() if not self.use_wtime else \
Clock(clock_type=ClockType.SYSTEM_TIME).now()
# time reset
if curr_rostime.nanoseconds == 0:
if len(self.get_times(topic=topic)) > 0:
print('time has reset, resetting counters')
self.set_times([], topic=topic)
return
curr = curr_rostime.nanoseconds
msg_t0 = self.get_msg_t0(topic=topic)
if msg_t0 < 0 or msg_t0 > curr:
self.set_msg_t0(curr, topic=topic)
self.set_msg_tn(curr, topic=topic)
self.set_times([], topic=topic)
else:
self.get_times(
topic=topic).append(curr - self.get_msg_tn(topic=topic))
self.set_msg_tn(curr, topic=topic)
if len(self.get_times(topic=topic)) > self.window_size:
self.get_times(topic=topic).pop(0)
def unblock_by_timeout():
nonlocal send_goal_future, goal, event
clock = Clock()
start_time = clock.now()
timeout = Duration.from_msg(goal.timeout)
timeout_padding = Duration(seconds=self.timeout_padding)
while not send_goal_future.done() and not event.is_set():
if clock.now() > start_time + timeout + timeout_padding:
break
# TODO(Anyone): We can't use Rate here because it would never expire
# with a single-threaded executor.
# See https://github.com/ros2/geometry2/issues/327 for ideas on
# how to timeout waiting for transforms that don't block the executor.
sleep(1.0 / self.check_frequency)
event.set()
def _convert_to_ros_time(field_type, field_value):
time = None
clock = Clock()
if field_type == 'time' and field_value == 'now':
time = clock.now()
else:
if field_type == 'time':
time = clock.now()
elif field_type == 'duration':
time = Duration()
if 'secs' in field_value:
setattr(time, 'secs', field_value['secs'])
if 'nsecs' in field_value:
setattr(time, 'nsecs', field_value['nsecs'])
return time
def drone_att_callback(self, rcv):
print("POSE: " + str(rcv))
self.lastPose = rcv
rcv.x *= math.pi / 180.0
rcv.y *= math.pi / 180.0
rcv.z *= math.pi / 180.0
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone_attitude"
quat = self.euler_to_quaternion(rcv.y, rcv.x, rcv.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish velocity
msg = Twist()
msg.linear.x = rcv.x
msg.angular.z = rcv.y
self.pub_vel.publish(msg)
def __init__(self, custom_qos_profile,freq,step):
super().__init__('variable_length_pub')
self.i=0
self.step = step
self.clock = Clock()
self.msg = ByteArray()
self.data = [0]*1024
if custom_qos_profile.reliability is QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE:
self.get_logger().info('Reliable publisher')
else:
self.get_logger().info('Best effort publisher')
self.pub = self.create_publisher(ByteArray, 'variable_length', qos_profile=custom_qos_profile)
timer_period = 1 / freq
self.tmr = self.create_timer(timer_period, self.timer_callback)
class VariableLengthPub(Node):
def __init__(self, custom_qos_profile,freq,step):
super().__init__('variable_length_pub')
self.i=0
self.step = step
self.clock = Clock()
self.msg = ByteArray()
self.data = [0]*1024
if custom_qos_profile.reliability is QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE:
self.get_logger().info('Reliable publisher')
else:
self.get_logger().info('Best effort publisher')
self.pub = self.create_publisher(ByteArray, 'variable_length', qos_profile=custom_qos_profile)
timer_period = 1 / freq
self.tmr = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
self.i += 1
self.msg.header.frame_id = str(self.i)
x = 0
#offset 24 bytes (size of header)
for x in range(1024*self.step):
self.msg.data.append(bytes(1))
self.get_logger().info('Publishing message: "{0}"'.format(self.msg.header.frame_id))
timestamp = self.clock.now()
self.msg.header.stamp = timestamp.to_msg()
self.pub.publish(self.msg)
def __init__(self, *, context: Context = None) -> None:
super().__init__()
self._context = get_default_context() if context is None else context
self._nodes: Set[Node] = set()
self._nodes_lock = RLock()
# Tasks to be executed (oldest first) 3-tuple Task, Entity, Node
self._tasks: List[Tuple[Task, Optional[WaitableEntityType],
Optional[Node]]] = []
self._tasks_lock = Lock()
# This is triggered when wait_for_ready_callbacks should rebuild the wait list
self._guard = GuardCondition(callback=None,
callback_group=None,
context=self._context)
# True if shutdown has been called
self._is_shutdown = False
self._work_tracker = _WorkTracker()
# Protect against shutdown() being called in parallel in two threads
self._shutdown_lock = Lock()
# State for wait_for_ready_callbacks to reuse generator
self._cb_iter = None
self._last_args = None
self._last_kwargs = None
# Executor cannot use ROS clock because that requires a node
self._clock = Clock(clock_type=ClockType.STEADY_TIME)
self._sigint_gc = SignalHandlerGuardCondition(context)
self._context.on_shutdown(self.wake)
class Throttle(LoggingFilter):
"""Ignore log calls if the last call is not longer ago than the specified duration."""
params = {
'throttle_duration_sec': None,
'throttle_time_source_type': Clock(),
}
@classmethod
def initialize_context(cls, context, **kwargs):
super(Throttle, cls).initialize_context(context, **kwargs)
context['throttle_last_logged'] = 0
if not isinstance(context['throttle_time_source_type'], Clock):
raise ValueError('Received throttle_time_source_type of "{0}" '
'is not a clock instance'.format(
context['throttle_time_source_type']))
@staticmethod
def should_log(context):
logging_condition = True
now = context['throttle_time_source_type'].now().nanoseconds
next_log_time = context['throttle_last_logged'] + (
context['throttle_duration_sec'] * 1e+9)
logging_condition = now >= next_log_time
if logging_condition:
context['throttle_last_logged'] = now
return logging_condition
def drone_odom_callback(self, rcv):
print("ODOM: " + str(rcv))
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone"
quat = self.euler_to_quaternion(self.lastPose.x, self.lastPose.y,
self.lastPose.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
position = Vector3()
position.x = rcv.x
position.y = rcv.y
position.z = rcv.z
msg.transform.translation = position
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish Rviz Path
msgpath = Path()
msgpath.header.frame_id = "/map"
msgpath.header.stamp = Clock().now().to_msg()
pose = PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = Clock().now().to_msg()
pose.pose.position.x = rcv.x
pose.pose.position.y = rcv.y
pose.pose.position.z = rcv.z
self.posearray.append(pose)
if len(self.posearray) > 50:
self.posearray = self.posearray[1:]
msgpath.poses = self.posearray
self.pub_posearray.publish(msgpath)
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 run(self, stat):
with self.lock:
clock = Clock(clock_type=ClockType.STEADY_TIME)
curtime = clock.now()
curseq = self.count
events = curseq - self.seq_nums[self.hist_indx]
window = (curtime - self.times[self.hist_indx]).nanoseconds * 1e-9
freq = events / window
self.seq_nums[self.hist_indx] = curseq
self.times[self.hist_indx] = curtime
self.hist_indx = (self.hist_indx + 1) % self.params.window_size
if events == 0:
stat.summary(b'2', 'No events recorded.')
elif freq < self.params.freq_bound['min'] * (
1 - self.params.tolerance):
stat.summary(b'1', 'Frequency too low.')
elif 'max' in self.params.freq_bound and freq > self.params.freq_bound['max'] *\
(1 + self.params.tolerance):
stat.summary(b'1', 'Frequency too high.')
else:
stat.summary(b'0', 'Desired frequency met')
stat.add('Events in window', '%d' % events)
stat.add('Events since startup', '%d' % self.count)
stat.add('Duration of window (s)', '%f' % window)
stat.add('Actual frequency (Hz)', '%f' % freq)
if 'max' in self.params.freq_bound and self.params.freq_bound['min'] == \
self.params.freq_bound['max']:
stat.add('Target frequency (Hz)',
'%f' % self.params.freq_bound['min'])
if self.params.freq_bound['min'] > 0:
stat.add(
'Minimum acceptable frequency (Hz)',
'%f' % (self.params.freq_bound['min'] *
(1 - self.params.tolerance)))
if 'max' in self.params.freq_bound:
stat.add(
'Maximum acceptable frequency (Hz)',
'%f' % (self.params.freq_bound['max'] *
(1 + self.params.tolerance)))
return stat
def tf_timer_callback(self):
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint"
msg.transform.translation = self.last_position
msg.transform.rotation = self.last_orientation
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
def __init__(self, node):
"""Construct an updater class."""
DiagnosticTaskVector.__init__(self)
self.node = node
self.publisher = self.node.create_publisher(DiagnosticArray,
'/diagnostics', 1)
self.clock = Clock()
now = self.clock.now()
self.last_time = now
self.last_time_period_checked = self.last_time
self.period_parameter = 'diagnostic_updater.period'
self.period = self.node.declare_parameter(self.period_parameter,
1.0).value
self.verbose = False
self.hwid = ''
self.warn_nohwid_done = False
def __init__(self, callback, callback_group, timer_period_ns):
# TODO(sloretz) Allow passing clocks in via timer constructor
self._clock = Clock(clock_type=ClockType.STEADY_TIME)
[self.timer_handle, self.timer_pointer
] = _rclpy.rclpy_create_timer(self._clock._clock_handle,
timer_period_ns)
self.timer_period_ns = timer_period_ns
self.callback = callback
self.callback_group = callback_group
# True when the callback is ready to fire but has not been "taken" by an executor
self._executor_event = False
def __init__(self, node):
super().__init__(ReentrantCallbackGroup())
self._clock = Clock(clock_type=ClockType.STEADY_TIME)
period_nanoseconds = 10000
self.timer = _rclpy.rclpy_create_timer(self._clock._clock_handle,
period_nanoseconds)[0]
self.timer_index = None
self.timer_is_ready = False
self.node = node
self.future = None
def drone_pose_callback(self, pose):
# Publish Rviz Path
msgpath = Path()
msgpath.header.frame_id = "/map"
msgpath.header.stamp = Clock().now().to_msg()
self.posearray.append(pose)
if len(self.posearray) > 50:
self.posearray = self.posearray[1:]
msgpath.poses = self.posearray
self.pub_posearray.publish(msgpath)
def test_sleep_until_time_in_past(default_context, clock_type):
clock = Clock(clock_type=clock_type)
sleep_duration = Duration(seconds=-1)
start = clock.now()
assert clock.sleep_until(clock.now() + sleep_duration)
stop = clock.now()
assert stop - start < A_SMALL_AMOUNT_OF_TIME
def testTimeStampStatus(self):
ts = TimeStampStatus()
stat = [DiagnosticStatusWrapper() for i in range(5)]
stat[0] = ts.run(stat[0])
clock = Clock(clock_type=ClockType.STEADY_TIME)
now = clock.now()
ts.tick((now.nanoseconds * 1e-9) + 2)
stat[1] = ts.run(stat[1])
now = clock.now()
ts.tick((now.nanoseconds * 1e-9))
stat[2] = ts.run(stat[2])
now = clock.now()
ts.tick((now.nanoseconds * 1e-9) - 4)
stat[3] = ts.run(stat[3])
now = clock.now()
ts.tick((now.nanoseconds * 1e-9) - 6)
stat[4] = ts.run(stat[4])
self.assertEqual(b'1', stat[0].level, 'no data should return a warning')
self.assertEqual(b'2', stat[1].level, 'too far future not reported')
self.assertEqual(b'0', stat[2].level, 'now not accepted')
self.assertEqual(b'0', stat[3].level, '4 seconds ago not accepted')
self.assertEqual(b'2', stat[4].level, 'too far past not reported')
self.assertEqual('', stat[0].name, 'Name should not be set by TimeStapmStatus')
self.assertEqual('Timestamp Status', ts.getName(), 'Name should be Timestamp Status')
