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)