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()
