def write_estimated_pose_timer_callback(self):
try:
transform_msg = self.tf_buffer.lookup_transform(
'map', 'base_link', Time())
orientation = transform_msg.transform.rotation
theta, _, _ = pyquaternion.Quaternion(
x=orientation.x,
y=orientation.y,
z=orientation.z,
w=orientation.w).yaw_pitch_roll
self.estimated_poses_df = self.estimated_poses_df.append(
{
't':
nanoseconds_to_seconds(
Time.from_msg(transform_msg.header.stamp).nanoseconds),
'x':
transform_msg.transform.translation.x,
'y':
transform_msg.transform.translation.y,
'theta':
theta
},
ignore_index=True)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pass
def test_headerless(self):
sub = Subscriber(self.node, String, "/empty")
cache = Cache(sub, 5, allow_headerless=False)
msg = String()
cache.add(msg)
self.assertIsNone(
cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)),
"Headerless message invalidly added.")
cache = Cache(sub, 5, allow_headerless=True)
cache.add(msg)
s = cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME))
self.assertEqual(s, msg, "invalid msg returned in headerless scenario")
currentRosTime = ROSClock().now()
s = cache.getElemAfterTime(currentRosTime)
self.assertIsNone(s, "invalid msg returned in headerless scenario")
cache.add(msg)
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
currentRosTime)
self.assertEqual(s, [msg],
"invalid msg returned in headerless scenario")
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
(currentRosTime + Duration(seconds=2)))
self.assertEqual(s, [msg, msg],
"invalid msg returned in headerless scenario")
def lookup_transform_full(
self,
target_frame: str,
target_time: Time,
source_frame: str,
source_time: Time,
fixed_frame: str,
timeout: Duration = Duration()
) -> TransformStamped:
"""
Get the transform from the source frame to the target frame using the advanced API.
:param target_frame: Name of the frame to transform into.
:param target_time: The time to transform to. (0 will get the latest)
:param source_frame: Name of the input frame.
:param source_time: The time at which source_frame will be evaluated. (0 will get the latest)
:param fixed_frame: Name of the frame to consider constant in time.
:param timeout: Time to wait for the target frame to become available.
:return: The transform between the frames.
"""
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.target_time = target_time.to_msg()
goal.fixed_frame = fixed_frame
goal.advanced = True
return self.__process_goal(goal)
def add(self, msg, my_queue, my_queue_index=None):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_approx')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn(
"can not use message filters for "
"messages without timestamp infomation when "
"'allow_headerless' is disabled. auto assign "
"ROSTIME to headerless messages once enabling "
"constructor option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# print(stamp)
self.lock.acquire()
my_queue[stamp.nanoseconds] = msg
while len(my_queue) > self.queue_size:
del my_queue[min(my_queue)]
# self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...]
if my_queue_index is None:
search_queues = self.queues
else:
search_queues = self.queues[:my_queue_index] + \
self.queues[my_queue_index+1:]
# sort and leave only reasonable stamps for synchronization
stamps = []
for queue in search_queues:
topic_stamps = []
for s in queue:
stamp_delta = Duration(nanoseconds=abs(s - stamp.nanoseconds))
if stamp_delta > self.slop:
continue # far over the slop
topic_stamps.append(
((Time(nanoseconds=s,
clock_type=stamp.clock_type)), stamp_delta))
if not topic_stamps:
self.lock.release()
return
topic_stamps = sorted(topic_stamps, key=lambda x: x[1])
stamps.append(topic_stamps)
for vv in itertools.product(*[list(zip(*s))[0] for s in stamps]):
vv = list(vv)
# insert the new message
if my_queue_index is not None:
vv.insert(my_queue_index, stamp)
qt = list(zip(self.queues, vv))
if (((max(vv) - min(vv)) < self.slop) and
(len([1 for q, t in qt if t.nanoseconds not in q]) == 0)):
msgs = [q[t.nanoseconds] for q, t in qt]
self.signalMessage(*msgs)
for q, t in qt:
del q[t.nanoseconds]
break # fast finish after the synchronization
self.lock.release()
def _get_earlist_index(self, index_list):
is_last = False
earlist_index = -1
index = 0
queue_list = []
for queue in self.fusion_operator.queues_for_input_ports.values():
queue_list.append(queue)
index = index + 1
index = 0
for queue in queue_list:
if index_list[index] != None:
if earlist_index < 0:
earlist_index = index
elif len(queue) > 0:
new = queue[index_list[index]]
new_stamp = new.header.stamp
new_stamp = Time.from_msg(new_stamp)
cur_queue = queue_list[earlist_index]
cur = cur_queue[index_list[earlist_index]]
cur_stamp = cur.header.stamp
cur_stamp = Time.from_msg(cur_stamp)
if (new_stamp < cur_stamp):
earlist_index = index
index = index + 1
if len(queue_list[earlist_index]) == index_list[earlist_index] + 1:
is_last = True
return earlist_index, is_last
def fusion_callback(self, channel, msg):
try:
start = time.time()
self.fusion_operator.queues_for_input_ports[channel].append(msg)
valid_input_data = self._find_valid_input_data()
oldest_birthmark = None
if valid_input_data:
for c, data in valid_input_data.items():
if data == None:
continue
if oldest_birthmark == None or Time.from_msg(
data.header.stamp) < Time.from_msg(
oldest_birthmark):
oldest_birthmark = data.header.stamp
freshness_constraint = data.freshness_constraint
valid_input_data[c] = pickle.loads(data.body)
splash_message = SplashMessage()
header = Header()
header.stamp = oldest_birthmark
splash_message.header = header
splash_message.body = array('B',
pickle.dumps(valid_input_data))
splash_message.freshness_constraint = freshness_constraint
self.fusion_operator.stream_output_ports[
self.fusion_operator.output_channel].write(splash_message)
else:
pass
print(f"time: {(time.time() - start) * 1000:.2f}ms")
except Exception as e:
print(traceback.format_exc())
print(e)
def _check_msg_time(self, foot_location: PointStamped) -> bool:
"""Checks if the foot_location given by CoViD is not older than
FOOT_LOCATION_TIME_OUT."""
msg_time = Time(
seconds=foot_location.header.stamp.sec,
nanoseconds=foot_location.header.stamp.nanosec,
)
current_time = Time(
seconds=self.gait_selection.get_clock().now().seconds_nanoseconds(
)[0],
nanoseconds=self.gait_selection.get_clock().now().
seconds_nanoseconds()[1],
)
time_difference = current_time - msg_time
self.logger.debug(
"Time difference between CoViD foot location and current time: "
f"{time_difference}.", )
if time_difference > FOOT_LOCATION_TIME_OUT:
self.logger.info(
"Foot location is more than 0.5 seconds old, time difference is "
f"{time_difference}. Stopping gait.", )
self._end = True
return True
return False
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 alignTimer(clock, timer):
# This function is a simple approximate alignment of a timer, it can be made smarter by compensating ns1
# for the time it takes to reset the timer. This would have to be done by running this
# algorithm a couple of times and take the avarage of the error and subtract this from ns1
dt = timer.timer_period_ns
time1 = clock.now()
ns = time1.nanoseconds
N, mod = divmod(ns, dt)
ns1 = (N + 1) * dt # time for which the timer should be reset
if ns == 0: # when sim_time_true, clock is always zero in the constructor... https://answers.ros.org/question/356733/ros2-why-rclcpp-node-now-function-returns-0-in-node-construtor/
t0 = Time(nanoseconds=0)
else:
while True: # wait until one dt has passed...
time2 = clock.now()
ns2 = time2.nanoseconds
if ns2 >= ns1: # reset timer
timer.reset()
break
t0 = Time(
nanoseconds=ns1
) # return the time for when the timer was supposed to be reset
return t0
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time):
print(f"Exporting camera {camera}")
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple([int(x) for x in util[f'S_rect_{camera_pad}']])
calib.distortion_model = 'plumb_bob'
calib.k = util[f'K_{camera_pad}']
calib.r = util[f'R_rect_{camera_pad}']
calib.d = [float(x) for x in util[f'D_{camera_pad}']]
calib.p = util[f'P_rect_{camera_pad}']
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path, f'image_{camera_pad}')
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.p = util[f'P{camera_pad}']
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg()
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = Time(seconds=dt).to_msg()
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr")
bag.create_topic(camera_info_topic)
camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr")
bag.create_topic(camera_topic)
bag.write((topic_name + topic_ext, serialize_message(image_message),
timestamp_to_nanoseconds_from_epoch(image_message.header.stamp)))
bag.write((topic_name + '/camera_info', serialize_message(calib),
timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
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 test_time_message_conversions_big_nanoseconds(self):
time1 = Time(nanoseconds=1553575413247045598, clock_type=ClockType.ROS_TIME)
builtins_msg = Builtins()
builtins_msg.time_value = time1.to_msg()
# Default clock type resulting from from_msg will be ROS time
time2 = Time.from_msg(builtins_msg.time_value)
assert isinstance(time2, Time)
assert time1 == time2
def on_message(self, message):
cls = self.__class__
# check if we need to authenticate
if cls.authenticate and not self.authenticated:
try:
if cls.bson_only_mode:
msg = bson.BSON(message).decode()
else:
msg = json.loads(message)
if msg['op'] == 'auth':
# check the authorization information
auth_srv_client = cls.node_handle.create_client(
Authentication, 'authenticate')
auth_srv_req = Authentication.Request()
auth_srv_req.mac = msg['mac']
auth_srv_req.client = msg['client']
auth_srv_req.dest = msg['dest']
auth_srv_req.rand = msg['rand']
auth_srv_req.t = Time(seconds=msg['t']).to_msg()
auth_srv_req.level = msg['level']
auth_srv_req.end = Time(seconds=msg['end']).to_msg()
while not auth_srv_client.wait_for_service(
timeout_sec=1.0):
cls.node_handle.get_logger().info(
'Authenticate service not available, waiting again...'
)
future = auth_srv_client.call_async(auth_srv_req)
rclpy.spin_until_future_complete(cls.node_handle, future)
# Log error if service could not be called.
if future.result() is not None:
self.authenticated = future.result().authenticated
else:
self.authenticated = False
cls.node_handle.get_logger.error(
'Authenticate service call failed')
if self.authenticated:
cls.node_handle.get_logger().info(
"Client {} has authenticated.".format(
self.protocol.client_id))
return
# if we are here, no valid authentication was given
cls.node_handle.get_logger().warn(
"Client {} did not authenticate. Closing connection.".
format(self.protocol.client_id))
self.close()
except:
# proper error will be handled in the protocol class
self.protocol.incoming(message)
else:
# no authentication required
self.protocol.incoming(message)
def test_update_subgait_start_delayed_did_start(self):
self.gait.start(Time(seconds=0), Duration(seconds=3))
gait_update = self.gait.update(Time(seconds=3.5))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=3))
self.assertEqual(self.gait._end_time, Time(seconds=4.5))
self.assertFalse(self.gait._start_is_delayed)
self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_not_done(self):
self.gait.start(Time(seconds=0))
gait_update = self.gait.update(Time(seconds=0.6))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=0))
self.assertEqual(self.gait._end_time, Time(seconds=1.5))
self.assertFalse(self.gait._scheduled_early)
self.assertEqual(gait_update, GaitUpdate.empty())
def test_triggered_time_jump_callbacks(self):
one_second = Duration(seconds=1)
half_second = Duration(seconds=0.5)
negative_half_second = Duration(seconds=-0.5)
negative_one_second = Duration(seconds=-1)
threshold1 = JumpThreshold(min_forward=one_second,
min_backward=negative_half_second,
on_clock_change=False)
threshold2 = JumpThreshold(min_forward=half_second,
min_backward=negative_one_second,
on_clock_change=False)
pre_callback1 = Mock()
post_callback1 = Mock()
pre_callback2 = Mock()
post_callback2 = Mock()
clock = ROSClock()
handler1 = clock.create_jump_callback(threshold1,
pre_callback=pre_callback1,
post_callback=post_callback1)
handler2 = clock.create_jump_callback(threshold2,
pre_callback=pre_callback2,
post_callback=post_callback2)
clock.set_ros_time_override(Time(seconds=1))
clock._set_ros_time_is_active(True)
pre_callback1.assert_not_called()
post_callback1.assert_not_called()
pre_callback2.assert_not_called()
post_callback2.assert_not_called()
# forward jump
clock.set_ros_time_override(Time(seconds=1.75))
pre_callback1.assert_not_called()
post_callback1.assert_not_called()
pre_callback2.assert_called()
post_callback2.assert_called()
pre_callback1.reset_mock()
post_callback1.reset_mock()
pre_callback2.reset_mock()
post_callback2.reset_mock()
# backwards jump
clock.set_ros_time_override(Time(seconds=1))
pre_callback1.assert_called()
post_callback1.assert_called()
pre_callback2.assert_not_called()
post_callback2.assert_not_called()
handler1.unregister()
handler2.unregister()
def test_time_message_conversions(self):
time1 = Time(nanoseconds=1, clock_type=ClockType.ROS_TIME)
builtins_msg = Builtins()
builtins_msg.time_value = time1.to_msg()
# Default clock type resulting from from_msg will be ROS time
time2 = Time.from_msg(builtins_msg.time_value)
assert isinstance(time2, Time)
assert time1 == time2
# Clock type can be specified if appropriate
time3 = Time.from_msg(builtins_msg.time_value, clock_type=ClockType.SYSTEM_TIME)
assert time3.clock_type == ClockType.SYSTEM_TIME
def main() -> int:
args = get_args()
infile = args.infile
if not os.path.exists(infile):
raise OSError("The input file does not exist: {}".format(infile))
outfile = args.outfile
if outfile is None:
outfile = infile.rstrip(".bag")
outfile += "-filtered.bag"
t0 = None
t1 = None
if args.start_time is not None:
t0 = Time(nanoseconds=args.start_time)
if args.end_time is not None:
t1 = Time(nanoseconds=args.end_time)
topic_list = None
if args.topics is not None:
topic_list = args.topics
topic_lut = {}
if args.map is not None:
for remapping in args.map:
from_to = remapping.split(":")
if len(from_to) == 2:
topic_lut[str(from_to[0])] = str(from_to[1])
with BagReader(infile) as inbag:
compress_opts = None
if ((not args.compress) and (not args.uncompress)):
compress_opts = inbag.comp_opt_
with BagWriter(outfile,
compress=args.compress,
storage_opts=inbag.s_opt_,
converter_opts=inbag.c_opt_,
compression_opts=compress_opts) as outbag:
for topic, msg, t, in inbag.read_messages(topics=topic_list,
start_time=t0,
end_time=t1):
out_topic = topic
if ((args.map is not None) and (topic in topic_lut)):
out_topic = topic_lut[topic]
outbag.write(out_topic, msg, t)
return 0
def test_start_delayed(self):
gait_update = self.gait.start(Time(seconds=0), Duration(seconds=1))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=1))
self.assertEqual(self.gait._end_time, Time(seconds=2.5))
self.assertTrue(self.gait._start_is_delayed)
self.assertEqual(
gait_update,
GaitUpdate.should_schedule_early(
self.gait._command_from_current_subgait()),
)
def test_start(self):
gait_update = self.gait.start(Time(seconds=0))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=0))
self.assertEqual(self.gait._end_time, Time(seconds=1.5))
self.assertFalse(self.gait._start_is_delayed)
self.assertEqual(
gait_update,
GaitUpdate.should_schedule(
self.gait._command_from_current_subgait()),
)
def test_update_subgait_done(self):
self.gait.start(Time(seconds=0))
gait_update = self.gait.update(Time(seconds=1.8))
self.assertEqual(self.gait.subgait_name, "left_swing")
self.assertEqual(self.gait._start_time, Time(seconds=1.8))
self.assertEqual(self.gait._end_time, Time(seconds=2.9))
self.assertFalse(self.gait._scheduled_early)
self.assertEqual(
gait_update,
GaitUpdate.should_schedule(
self.gait._command_from_current_subgait()),
)
def read_data_frames(bag_view: BagView,
field_dict: Dict[str, List[str]],
auto_stamp=True):
data = {}
field_types = {}
topics = dict(bag_view.topics())
for (topic, fields) in field_dict.items():
data[topic] = {}
field_types[topic] = {}
msg_type = topics[topic]
fields_and_types = msg_type.get_fields_and_field_types()
if ('header.stamp' not in fields and auto_stamp
and 'header' in fields_and_types
and fields_and_types['header'] == 'std_msgs/Header'):
fields = fields.copy() # ensure not to mutate the input
fields.append('header.stamp')
for field in fields:
field_types[topic][field] = _field_type(msg_type, field)
data[topic][field] = []
for (topic, msg, _) in bag_view:
for field, field_type in field_types[topic].items():
value = _rgetattr(msg, field)
if field_type == 'builtin_interfaces/Time':
ns = Time.from_msg(value).nanoseconds
value = pd.to_datetime(ns, origin='unix', unit='ns')
data[topic][field].append(value)
return {
topic: pd.DataFrame(data_dict)
for topic, data_dict in data.items()
}
def _odom_callback(self, msg):
try:
# Get latest transform
tf = self._tf_buffer.lookup_transform(
"map", "base_link", Time(), timeout=Duration(seconds=1.0))
self._odom_pose_stamped.header = msg.header
self._odom_pose_stamped.pose = msg.pose.pose
tf_pose = tf2_geometry_msgs.do_transform_pose(
self._odom_pose_stamped, tf)
except LookupException:
self.get_logger().warn(
"Failed to lookup_transform from map to odom")
return
except ConnectivityException:
self.get_logger().warn(
"ConnectivityException, 'map' and 'base_link' are not part of the same tree"
)
return
self._position = (tf_pose.pose.position.x, tf_pose.pose.position.y)
self._orientation = self.quaternion_to_euler(
tf_pose.pose.orientation.x,
tf_pose.pose.orientation.y,
tf_pose.pose.orientation.z,
tf_pose.pose.orientation.w,
)
def callback_delay(self, msg):
"""
Calculate delay time.
:param msg: Message instance
"""
if not hasattr(msg, 'header'):
raise RuntimeError('msg does not have header')
with self.lock:
curr_rostime = self._clock.now()
# time reset
if curr_rostime.nanoseconds == 0:
if len(self.delays) > 0:
print('time has reset, resetting counters')
self.delays = []
return
curr = curr_rostime.nanoseconds
if self.msg_t0 < 0 or self.msg_t0 > curr:
self.msg_t0 = curr
self.msg_tn = curr
self.delays = []
else:
# store the duration nanoseconds in self.delays
duration = (curr_rostime - Time.from_msg(msg.header.stamp))
self.delays.append(duration.nanoseconds)
self.msg_tn = curr
if len(self.delays) > self.window_size:
self.delays.pop(0)
def _task(self):
if len(self._queue) > 0:
# print("*"*5, len(self._queue))
msg = self._queue.pop(0)
# self._count_queue.append(msg)
self.count += 1
self.count_rate += 1
time_diff_ms = (
self._stream_output_port.parent.get_clock().now().nanoseconds -
Time.from_msg(msg.header.stamp).nanoseconds) / 1000000
if msg.freshness > 0 and msg.freshness < time_diff_ms:
self.count_fresh += 1
# raise self.exception('{}ms exceeded(constraint: {}ms, cur: {}ms'.format(time_diff_ms - msg.freshness, msg.freshness, time_diff_ms))
self._task()
return
tlqkf1 = time.time()
self.count_publish += 1
# print("rate publish: ", tlqkf1 - self.tlqkf1_prev)
self._publisher.publish(msg)
self.tlqkf1_prev = tlqkf1
elif self._last_sent_item:
self.count += 1
msg = self._last_sent_item
else:
return
self._last_sent_item = msg
def add(self, msg):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_cache')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn("can not use message filters messages "
"without timestamp infomation when "
"'allow_headerless' is disabled. "
"auto assign ROSTIME to headerless "
"messages once enabling constructor "
"option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# Insert sorted
self.cache_times.append(stamp)
self.cache_msgs.append(msg)
# Implement a ring buffer, discard older if oversized
if (len(self.cache_msgs) > self.cache_size):
del self.cache_msgs[0]
del self.cache_times[0]
# Signal new input
self.signalMessage(msg)
def update_state(self, msg: AsebaEvent) -> None:
if self.state == CalibrationState.waiting_button:
return
if self.state == CalibrationState.waiting_ground:
self.ths = [d - self.gap for d in msg.data]
self.state = CalibrationState.running
return
if self.motor_speed == 0:
return
line = self.line
if self.motor == 'right':
v = msg.data[0]
th = self.ths[0]
else:
v = msg.data[1]
th = self.ths[1]
self.get_logger().info(f'update_state {line.state} {v} {th} -> ?')
stamp = Time.from_msg(msg.stamp).nanoseconds * 1e-9
if v > th and line.state == LineState.UNKNOWN:
line.state = LineState.OFF
if v < th and line.state == LineState.OFF:
line.state = LineState.IN
line.in_at_time = stamp
if v > th and line.state == LineState.IN:
line.state = LineState.OFF
pick = (stamp + line.in_at_time) / 2
self.get_logger().info(f"Line after {pick} s")
self.add_pick(pick)
def test_duration_comparators(self):
duration1 = Duration(nanoseconds=1)
duration2 = Duration(nanoseconds=2)
self.assertFalse(duration1 == duration2)
self.assertTrue(duration1 != duration2)
self.assertFalse(duration1 > duration2)
self.assertFalse(duration1 >= duration2)
self.assertTrue(duration1 < duration2)
self.assertTrue(duration1 <= duration2)
duration1 = Duration(nanoseconds=5e9)
duration2 = Duration(seconds=5)
self.assertTrue(duration1 == duration2)
# Invalid combinations
duration1 = Duration(nanoseconds=1)
with self.assertRaises(TypeError):
duration1 == 1
time = Time()
with self.assertRaises(TypeError):
duration1 == time
with self.assertRaises(TypeError):
duration1 != time
with self.assertRaises(TypeError):
duration1 > time
with self.assertRaises(TypeError):
duration1 >= time
with self.assertRaises(TypeError):
duration1 < time
with self.assertRaises(TypeError):
duration1 <= time
def lookup_transform(
self,
target_frame: str,
source_frame: str,
time: Time,
timeout: Duration = Duration()
) -> TransformStamped:
"""
Get the transform from the source frame to the target frame.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform (0 will get the latest).
:param timeout: Time to wait for the target frame to become available.
:return: The transform between the frames.
"""
if isinstance(time, builtin_interfaces.msg.Time):
source_time = Time.from_msg(time)
warnings.warn(
'Passing a builtin_interfaces.msg.Time argument is deprecated, and will be removed in the near future. '
'Use rclpy.time.Time instead.')
elif isinstance(time, Time):
source_time = time
else:
raise TypeError('Must pass a rclpy.time.Time object.')
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.advanced = False
return self.__process_goal(goal)
def now(self):
from rclpy.time import Time
time_handle = _rclpy.rclpy_clock_get_now(self._clock_handle)
# TODO(dhood): Return a python object from the C extension
return Time(
nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle),
clock_type=self.clock_type)
