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')
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 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 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 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 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 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 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 _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
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 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 test_clock_now(self):
# System time should be roughly equal to time.time()
# There will still be differences between them, with the bound depending on the scheduler.
clock = Clock(clock_type=ClockType.SYSTEM_TIME)
now = clock.now()
python_time_sec = time.time()
assert isinstance(now, Time)
assert abs(now.nanoseconds * 1e-9 - python_time_sec) < 5
# Unless there is a date change during the test, system time have increased between these
# calls.
now2 = clock.now()
assert now2 > now
# Steady time should always return increasing values
clock = Clock(clock_type=ClockType.STEADY_TIME)
now = clock.now()
now2 = now
for i in range(10):
now2 = clock.now()
assert now2 > now
now = now2
def test_sleep_for_context_shut_down(non_default_context):
clock = Clock()
retval = None
def run():
nonlocal retval
retval = clock.sleep_for(Duration(seconds=10),
context=non_default_context)
t = threading.Thread(target=run)
t.start()
# wait for thread to get inside sleep_for call
time.sleep(0.2)
non_default_context.shutdown()
# 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 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
class HeartbeatPublisher(Node):
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 timer_callback(self):
timestamp = self._clock.now()
msg = Header()
msg.stamp = timestamp.to_msg()
self._pub.publish(msg)
self.get_logger().debug('Publishing: "%s"' % msg.stamp)
def tick(self, stamp):
"""
Signal an event.
@param stamp The timestamp of the event that will be used in computing
intervals. Can be either a double or a ros::Time.
"""
if not isinstance(stamp, float):
stamp = stamp * 1e9
with self.lock:
if stamp == 0:
self.zero_seen = True
else:
clock = Clock(clock_type=ClockType.STEADY_TIME)
delta = clock.now().nanoseconds - stamp * 1e9
delta = delta * 1e-9
if not self.deltas_valid or delta > self.max_delta:
self.max_delta = delta
if not self.deltas_valid or delta < self.min_delta:
self.min_delta = delta
self.deltas_valid = True
def visualize(
publisher: Publisher,
clock: Clock,
path_id: int,
path: Union[np.ndarray, Iterable[List[float]]], # n x 2
weights: Optional[Union[List[float], np.ndarray]] = None,
weight_max: Optional[float] = 1.0,
frame_id: Optional[str] = "/map",
ns: Optional[str] = "paths",
scale: Optional[float] = 0.05,
color=ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0)
) -> None:
"""
Generates a visualization_msgs::msg::Marker from a set of points
Can colorize points on a scale based on another set of values ('weights')
"""
# Use point cloud instead?
m = Marker(header=Header(frame_id=frame_id, stamp=clock.now().to_msg()),
ns=ns,
action=Marker.ADD,
id=path_id,
type=Marker.LINE_STRIP if len(path) > 1 else Marker.POINTS,
scale=Vector3(x=scale, y=scale),
color=color)
m.points.extend(Point(x=1.0 * pt[0], y=1.0 * pt[1]) for pt in path)
if weights is not None:
assert len(path) == len(weights)
m.colors.extend(
# TODO: Better color range generation
ColorRGBA(r=1.0,
g=1.0 - min(max(1.0 * wt / weight_max, 0.0), 1.0),
b=0.0,
a=1.0) for wt in weights)
publisher.publish(m)
sleep(0.01)
class NeopixelNode(Node):
WHITE = (255, 255, 255)
RED = (255, 0, 0)
BLUE = (0, 0, 255)
OFF = (0, 0, 0)
DEFAULT_BRIGHTNESS = 1.0
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 _standby_light(self):
self.pixels.fill(self.OFF)
color = tuple(
map(lambda x: int(x * 0.01),
(self.BLUE if self._connected else self.RED)))
for i in range(0, len(self.pixels), 2):
self.pixels[i] = color
def _update_light(self):
if self._enabled and self._connected:
self.pixels.fill(self.RED)
else:
self._standby_light()
def _set_connected(self, value):
if value is self._connected:
return
self.get_logger().debug('connected: {}'.format(value))
self._connected = value
self._update_light()
def _enable_callback(self, msg):
self.get_logger().debug('Enable received {}'.format(msg.data))
self._enabled = msg.data
self._update_light()
def _heartbeat_callback(self, msg):
self.get_logger().debug('Heartbeat received {}'.format(msg.stamp))
current = Time.from_msg(self._clock.now().to_msg())
if (current - Time.from_msg(msg.stamp)) > self._timeout_duration:
return
self._set_connected(True)
self._timer.reset()
def _timer_callback(self):
self._timer.cancel()
self._set_connected(False)
def test_sleep_until_non_default_context(non_default_context):
clock = Clock()
assert clock.sleep_until(clock.now() + Duration(seconds=0.1),
context=non_default_context)
def test_sleep_until_invalid_context():
clock = Clock()
with pytest.raises(NotInitializedException):
clock.sleep_until(clock.now() + Duration(seconds=0.1),
context=Context())
class Updater(DiagnosticTaskVector):
"""
Manage a list of diagnostic tasks, and calls them in a rate-limited manner.
This class manages a list of diagnostic tasks. Its update function
should be called frequently. At some predetermined rate, the update
function will cause all the diagnostic tasks to run, and will collate
and publish the resulting diagnostics. The publication rate is
determined by the "~diagnostic_period" ros parameter.
The class also allows an update to be forced when something significant
has happened, and allows a single message to be broadcast on all the
diagnostics if normal operation of the node is suspended for some
reason.
"""
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 update(self):
"""Causes the diagnostics to update if the inter-update interval has been exceeded."""
self._check_diagnostic_period()
now = self.clock.now()
if now >= self.last_time:
self.force_update()
def force_update(self):
"""
Force the diagnostics to update.
Useful if the node has undergone a drastic state change that should be
published immediately.
"""
self.last_time = self.clock.now()
warn_nohwid = len(self.hwid) == 0
status_vec = []
with self.lock: # Make sure no adds happen while we are processing here.
for task in self.tasks:
status = DiagnosticStatusWrapper()
status.level = b'2'
status.name = task.name
status.message = 'No message was set'
status.hardware_id = self.hwid
status = task.run(status)
status_vec.append(status)
if status.level:
warn_nohwid = False
if self.verbose and status.level:
self.node.get_logger().warn(
'Non-zero diagnostic status. Name: %s, status\
%i: %s' %
(status.name, status.level, status.message))
if warn_nohwid and not self.warn_nohwid_done:
self.node.get_logger().warn(
'diagnostic_updater: No HW_ID was set. This is probably\
a bug. Please report it. For devices that do not have a\
HW_ID, set this value to none. This warning only occurs\
once all diagnostics are OK so it is okay to wait until\
the device is open before calling setHardwareID.'
)
self.warn_nohwid_done = True
self.publish(status_vec)
def broadcast(self, lvl, msg):
"""
Output a message on all the known DiagnosticStatus.
Useful if something drastic is happening such as shutdown or a self-test.
@param lvl Level of the diagnostic being output.
@param msg Status message to output.
"""
status_vec = []
for task in self.tasks:
status = DiagnosticStatusWrapper()
status.name = task.name
status.summary(lvl, msg)
status_vec.append(status)
self.publish(status_vec)
def setHardwareID(self, hwid):
self.hwid = hwid
def _check_diagnostic_period(self):
"""Recheck the diagnostic_period on the parameter server."""
# This was getParamCached() call in the cpp code. i.e. it would throttle
# the actual call to the parameter server using a notification of change
# mechanism.
# This is not available in rospy. Hence I throttle the call to the
# parameter server using a standard timeout mechanism (4Hz)
now = self.clock.now()
if now >= self.last_time_period_checked:
self.period = self.node.get_parameter(self.period_parameter).value
self.last_time_period_checked = now
def publish(self, msg):
"""Publish a single diagnostic status or a vector of diagnostic statuses."""
if not type(msg) is list:
msg = [msg]
for stat in msg:
stat.name = self.node.get_name() + ': ' + stat.name
now = self.clock.now()
da = DiagnosticArray()
db = DiagnosticStatus()
db.name = stat.name
db.message = stat.message
db.hardware_id = stat.hardware_id
db.values = stat.values
da.status.append(db)
da.header.stamp = now.to_msg() # Add timestamp for ROS 0.10
self.publisher.publish(da)
def addedTaskCallback(self, task):
stat = DiagnosticStatusWrapper()
stat.name = task.name
stat.summary(b'0', 'Node starting up')
self.publish(stat)
