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 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 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_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
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 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 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 __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)
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 __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 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 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_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 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 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 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 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, 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 __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 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 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 __init__(self, node): super().__init__(ReentrantCallbackGroup()) self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 with self._clock.handle, node.context.handle: self.timer = _rclpy.Timer(self._clock.handle, node.context.handle, period_nanoseconds) self.timer_index = None self.timer_is_ready = False self.node = node self.future = None
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 _extract_robot_pose(self, color, detection_robots, time_stamp): # ロボット姿勢を抽出する # ロボット姿勢が複数ある場合は、平均値を姿勢とする detections = [[] for i in range(len(self._robot_info[color]))] # ID毎のリストに分ける for robot in detection_robots: robot_id = robot.robot_id detections[robot_id].append(robot) for robot_id, robots in enumerate(detections): if robots: average_pose = self._average_pose(robots) velocity = self._velocity( self._robot_info[color][robot_id].pose, average_pose, self._robot_info[color][robot_id].detection_stamp, time_stamp) self._robot_info[color][robot_id].robot_id = robot_id self._robot_info[color][robot_id].pose = average_pose self._robot_info[color][ robot_id].last_detection_pose = average_pose self._robot_info[color][robot_id].velocity = velocity self._robot_info[color][robot_id].detected = True self._robot_info[color][robot_id].detection_stamp = time_stamp self._robot_info[color][robot_id].disappeared = False else: self._robot_info[color][robot_id].detected = False self._robot_info[color][robot_id].robot_id = robot_id # 座標を受け取らなかった場合は、速度を用いて線形予測する if self._robot_info[color][robot_id].disappeared is False: duration = Clock(clock_type=ClockType.ROS_TIME).now() - \ Time.from_msg(self._robot_info[color][robot_id].detection_stamp) diff_time_secs = duration.nanoseconds / 1000000000 self._robot_info[color][robot_id].pose = self._estimate( self._robot_info[color][robot_id].last_detection_pose, self._robot_info[color][robot_id].velocity, diff_time_secs) # 一定時間、座標を受け取らなかったら消滅判定にする if diff_time_secs > self._DISAPPERED_TIME_THRESH: self._robot_info[color][robot_id].disappeared = True if self._PUBLISH_ROBOT[color]: for robot_id in range(len(self._pubs_robot_info[color])): self._pubs_robot_info[color][robot_id].publish( self._robot_info[color][robot_id])
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 timer_callback(self): self.marker.header.stamp = Clock().now().to_msg() self.shark.header.stamp = Clock().now().to_msg() self.shark_heading += 0.3 self.shark_x += math.sin(self.shark_heading) self.shark_y -= math.cos(self.shark_heading) cy = math.cos(self.shark_heading * 0.5) sy = math.sin(self.shark_heading * 0.5) cr = math.cos(0 * 0.5) sr = math.sin(0 * 0.5) cp = math.cos(0 * 0.5) sp = math.sin(0 * 0.5) self.shark.pose.position.x = self.shark_x self.shark.pose.position.y = self.shark_y self.shark.pose.orientation.w = cy * cr * cp + sy * sr * sp self.shark.pose.orientation.x = cy * sr * cp - sy * cr * sp self.shark.pose.orientation.y = cy * cr * sp + sy * sr * cp self.shark.pose.orientation.z = sy * cr * cp - cy * sr * sp self.pub.publish(self.marker) self.shark_pub.publish(self.shark)
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 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()