i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 04:29:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap_demos tests and docs (#1462)

Browse Source 
* rtabmap_demos tests and docs

* added bag testing

* added rtabmap_examples launch tests

* updating demo bag download paths

* added netherdrone demo

* Fixed rgb-only callback with lidar rejected.  Updated lidar params

* Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping

* fixing ci

* lidar demo added intermediate_nodes option

* added netherdrone as demo test

* running rtabmap_demos tests on ci

* added rtabmap_launch tests, fixed ground_truth_base_frame_id usage

* ficing rolling

* updating demo test harnest

* densify golden trajectories to avoid tf missing

* fixing tf steps

* updated min icp ratio for netherdrone demo

* fixing image_transport arg->params

* export pose opt=0

* lets process all frames

* updated netherdrone golden

* fixing publishers queue size just for tests

* added playdback demo doc

* Adding more logs to debug ci

* Fixing QOS for CI to reliable, added find-object demo test

* name threads

* fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle

* 30 Hz polling backward comp

* fixing test tf sim lock

* fixing lyrical qos bag parsing

* faster replay

* lockstep

* fixing clock deadlock

* Added test on shutdown

* updated netherdrone golden poses

* Extended stereo outdoor test

* updated shutdown test

* updated test

* multi-thread flaky test

* adding backtrace when test fails

* increased closure slack for netherdrone

* g2o gauss newton on stereo

* adjusted maximum optimizer iterations

* updated default iterations

* Added netherdrone in list of demos
This commit is contained in:
matlabbe
2026-10-10 12:23:49 -07:00
committed by GitHub
parent cb2117f1ed
commit 82f0754bf7
80 changed files with 18405 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_demos/test/bag_lockstep.py
+496
View File
@@ -0,0 +1,496 @@
"""
Play a rosbag2 bag in lockstep with the nodes consuming it.
`ros2 bag play` publishes at the bag's pace whatever the consumers do; on a loaded
machine a node still busy with the previous frame drops the next one, and which
frames it drops changes from run to run. Here a sensor message is only published
once every process of the pipeline is idle again, so on a slower machine the replay
just takes longer, and each node sees every message it would on an idle one.
This needs every publisher of the pipeline, this player's included, to send the
message before publish() returns (RMW_FASTRTPS_PUBLICATION_MODE=SYNCHRONOUS with Fast
DDS, which otherwise sends large messages from a thread of its own; Cyclone DDS always
does). Else, right after a publish(), nothing has woken up yet and all looks idle.
"Idle" is read from the kernel, not from the nodes: a thread with work to do is
runnable (state R) -- also while it waits for a CPU, which is what makes this hold
under load -- and a node waiting for its next message has every thread sleeping. A
node passing a message on wakes the next one before going back to sleep, so a
pipeline is idle only once its last stage is done. That needs no knowledge of what
each node publishes, or of which frames the SLAM node keeps (those it skips because
of Rtabmap/DetectionRate publish nothing at all).
TF is not gated. It is published ahead of the sensor data by a lookahead, so a
transform interpolated at a frame's stamp -- or at the stamp of a lidar sweep's last
point, for deskewing -- is already in the listener's buffer when the frame arrives.
Transforms not in the bag can be added to its TF (add_trajectory), each sent a lead
time before its stamp: a ground truth trajectory for rtabmap, for instance.
"""
import collections
import heapq
import os
import time
from typing import Callable, Dict, List, Optional, Set, Tuple
import numpy as np
import rosbag2_py
import yaml
from geometry_msgs.msg import TransformStamped
from rclpy.node import Node
from rclpy.serialization import serialize_message
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
from rosgraph_msgs.msg import Clock
from rosidl_runtime_py.utilities import get_message
from scipy.spatial.transform import Rotation, Slerp
from tf2_msgs.msg import TFMessage
# Only the large messages, those the pipeline takes time to process, wait for it to be
# idle: images, point clouds and laser scans. The others (IMU, camera info, odometry,
# TF...) go out in the bag's order without waiting, and wait in their subscribers'
# queues if need be: before the next large one, the pipeline is idle again, so it has
# processed them all.
GATED_TYPES = ('sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage',
'sensor_msgs/msg/PointCloud2', 'sensor_msgs/msg/LaserScan')
def _recorded_transient_local(offered_qos_profiles) -> bool:
"""Whether a topic was recorded with a TRANSIENT_LOCAL publisher.
rosbag2_py gives the recorded profiles as YAML text (humble; the durability as a
number, 1, or a name, transient_local, depending on the bag's version), or as its own
QoS objects (lyrical), whose durability() only sets it: those are converted to rclpy's.
"""
profiles = offered_qos_profiles
if isinstance(profiles, str):
profiles = yaml.safe_load(profiles) if profiles.strip() else []
for profile in profiles or []:
if isinstance(profile, dict):
durability = profile.get('durability')
else:
if not isinstance(profile, QoSProfile) and hasattr(rosbag2_py, 'convert_rclcpp_qos_to_rclpy_qos'):
profile = rosbag2_py.convert_rclcpp_qos_to_rclpy_qos(profile)
durability = getattr(profile, 'durability', None)
if durability in (1, DurabilityPolicy.TRANSIENT_LOCAL) or str(durability).lower() in (
'transient_local', 'durabilitypolicy.transient_local'):
return True
return False
def _read_stat(path: str):
with open(path) as f:
stat = f.read()
# The command name is in parentheses and may itself hold spaces or parentheses.
name = stat[stat.index('(') + 1:stat.rindex(')')]
return name, stat[stat.rindex(')') + 2:].split()
def _read_next(reader):
"""The next message of @p reader: topic, serialized data and receive timestamp.
read_next_ext() (Lyrical and later) also returns the send timestamp; read_next(),
deprecated there, warns at each message.
"""
if hasattr(reader, 'read_next_ext'):
return reader.read_next_ext()[:3]
return reader.read_next()
class ProcessTree:
"""The processes started under a root process (the `ros2 launch` one)."""
def __init__(self, root_pid: int):
self.root_pid = root_pid
self._pids: List[int] = []
self._refreshed = 0.0
# Each thread's stat file, kept open: re-reading it (pread) is a fraction of the
# cost of opening it again, at each poll, for hundreds of threads.
self._stat_fds: Dict[Tuple[int, str], int] = {}
def pids(self) -> List[int]:
# Nodes start in the first seconds and component containers may spawn later;
# walking /proc is the expensive part, so it is not done on every poll.
if time.monotonic() - self._refreshed > 1.0:
children: Dict[int, List[int]] = {}
for entry in os.listdir('/proc'):
if entry.isdigit():
try:
_, fields = _read_stat(f'/proc/{entry}/stat')
except (OSError, ValueError):
continue
children.setdefault(int(fields[1]), []).append(int(entry))
pids, todo = [], [self.root_pid]
while todo:
for child in children.get(todo.pop(), []):
pids.append(child)
todo.append(child)
self._pids = pids
self._refreshed = time.monotonic()
return self._pids
def busy_threads(self) -> List[str]:
"""Threads of the tree's processes (not of the root) that have work to do."""
busy = []
alive = set()
for pid in self.pids():
try:
# Listed at every poll (cheap): a thread started since is watched at once.
tids = os.listdir(f'/proc/{pid}/task')
except OSError:
continue
for tid in tids:
key = (pid, tid)
alive.add(key)
fd = self._stat_fds.get(key)
try:
if fd is None:
fd = os.open(f'/proc/{pid}/task/{tid}/stat', os.O_RDONLY)
self._stat_fds[key] = fd
stat = os.pread(fd, 512, 0)
except OSError:
continue
# The state follows the command name, which is in parentheses and may
# itself hold spaces or parentheses.
end = stat.rfind(b')')
if end < 0 or end + 2 >= len(stat):
continue
state = chr(stat[end + 2])
# R: running or runnable. D: in an uninterruptible wait, writing the
# database for instance -- busy as well. S, I, T, Z: nothing to do.
if state in ('R', 'D'):
name = stat[stat.find(b'(') + 1:end].decode(errors='replace')
busy.append(f'{pid}/{name}:{state}')
for key in [k for k in self._stat_fds if k not in alive]:
os.close(self._stat_fds.pop(key))
return busy
def wait_idle(self, idle_polls: int = 2, period: float = 0.002, timeout: float = 300.0):
"""Wait until idle_polls polls in a row see no busy thread.
A single quiet poll is not enough: timers wake threads briefly, and a message
in flight between two nodes leaves a gap of a few microseconds where both
sleep. Two, a period apart, cover a few milliseconds; a longer gap seen as idle
only lets the next message out early, to wait in its subscriber's queue. Raises
TimeoutError, naming the busy threads, if never idle.
"""
deadline = time.monotonic() + timeout
quiet = 0
polls = 0
# How often each thread was seen busy: one that keeps waking up (a timer) can keep
# the pipeline from ever looking idle long enough, without being busy at the last
# poll.
seen = collections.Counter()
while quiet < idle_polls:
if time.monotonic() > deadline:
most = ', '.join(f'{name} ({count})' for name, count in seen.most_common(5))
raise TimeoutError(
f'never idle for {idle_polls} polls in a row after {timeout:.0f} s; '
f'busy most often, of {polls} polls: {most}')
busy = self.busy_threads()
polls += 1
seen.update(busy)
quiet = 0 if busy else quiet + 1
if quiet < idle_polls:
time.sleep(period)
class LockstepPlayer:
"""Publish a bag's messages, each sensor message once the pipeline is idle.
Several bags (a list) are played one after the other, as `ros2 bag play` of each in
turn would: the recordings of a session split in parts.
"""
def __init__(self, node: Node, bag_dir, tree: ProcessTree, lookahead: float = 0.2):
self.node = node
self.bag_dirs = [bag_dir] if isinstance(bag_dir, str) else list(bag_dir)
self.tree = tree
self.lookahead = lookahead
self.gated: Set[str] = set()
self._publishers = {}
self._types = {}
self._clock = node.create_publisher(Clock, '/clock', 10)
self._extra = [] # (release time, serialized TFMessage)
self._extra_tf = node.create_publisher(
TFMessage, '/tf', QoSProfile(depth=100, history=HistoryPolicy.KEEP_LAST,
reliability=ReliabilityPolicy.RELIABLE))
metas = {}
for bag in self.bag_dirs:
for meta in self._open(bag).get_all_topics_and_types():
metas.setdefault(meta.name, meta)
for meta in metas.values():
name = meta.name if meta.name.startswith('/') else '/' + meta.name
# Durability as recorded, as ros2 bag play does: a subscriber asking for
# TRANSIENT_LOCAL is not matched with a VOLATILE publisher (image_proc's, for
# camera info, with Fast DDS since lyrical). Always reliable, and deep, to
# lose nothing.
transient = name == '/tf_static' or _recorded_transient_local(meta.offered_qos_profiles)
qos = QoSProfile(depth=100, history=HistoryPolicy.KEEP_LAST,
reliability=ReliabilityPolicy.RELIABLE,
durability=(DurabilityPolicy.TRANSIENT_LOCAL if transient
else DurabilityPolicy.VOLATILE))
self._publishers[meta.name] = (
name, node.create_publisher(get_message(meta.type), name, qos))
self._types[meta.name] = meta.type
def add_trajectory(self, frame_id: str, child_frame_id: str, trajectory,
step: float = 0.25):
"""Publish (stamp, (x y z qx qy qz qw)) poses as frame_id -> child_frame_id.
The first and last poses are held to the bags' start and end, so that no lookup
falls outside of it: one would make the node wait for the transform, asleep,
looking idle. Poses are then added, interpolated as TF would (linearly, and by
slerp for the rotation), so that none are more than `step` seconds apart: TF
returns the same from them at any stamp.
TF interpolates between two samples, so a lookup at a stamp needs the sample after
it: each is sent 2 * step ahead of its stamp. A node may still look a stamp up
late (rtabmap, an intermediate node when the next node with data arrives) and a
TF buffer keeps 10 s: the trajectory's own gaps can be several seconds (a robot
standing still adds no nodes), and a lead covering them would leave it no slack.
"""
spans = []
for bag in self.bag_dirs:
metadata = self._open(bag).get_metadata()
begin = metadata.starting_time.nanoseconds / 1e9
spans.append((begin, begin + metadata.duration.nanoseconds / 1e9))
start, end = min(s for s, _ in spans), max(e for _, e in spans)
trajectory = sorted(trajectory)
if not trajectory:
return
trajectory = [(start, trajectory[0][1])] + trajectory + [(end, trajectory[-1][1])]
stamps = np.array([stamp for stamp, _ in trajectory])
poses = np.array([pose for _, pose in trajectory])
stamps, unique = np.unique(stamps, return_index=True)
poses = poses[unique]
dense = np.union1d(stamps, np.arange(stamps[0], stamps[-1], step))
xyz = np.stack([np.interp(dense, stamps, poses[:, k]) for k in range(3)], axis=1)
quaternions = Slerp(stamps, Rotation.from_quat(poses[:, 3:]))(dense).as_quat()
lead = 2 * step
for stamp, (x, y, z), (qx, qy, qz, qw) in zip(dense, xyz, quaternions):
ns = int(round(stamp * 1e9))
msg = TransformStamped()
msg.header.stamp = _to_time(ns)
msg.header.frame_id = frame_id
msg.child_frame_id = child_frame_id
t, r = msg.transform.translation, msg.transform.rotation
t.x, t.y, t.z, r.x, r.y, r.z, r.w = x, y, z, qx, qy, qz, qw
self._extra.append(
(ns - int(lead * 1e9), serialize_message(TFMessage(transforms=[msg]))))
@staticmethod
def _open(bag_dir: str):
reader = rosbag2_py.SequentialReader()
reader.open(rosbag2_py.StorageOptions(uri=bag_dir, storage_id='sqlite3'),
rosbag2_py.ConverterOptions('cdr', 'cdr'))
return reader
def connect(self, required: List[str], timeout: float = 60.0):
"""Wait for the pipeline to subscribe, then choose which topics to gate.
A reliable publisher still loses what it publishes before discovery has
matched it with a subscriber. Only topics something subscribes to are gated:
waiting after a message nobody reads would only slow the replay down.
"""
deadline = time.monotonic() + timeout
by_name = {name: pub for name, pub in self._publishers.values()}
for topic in required:
while by_name[topic].get_subscription_count() == 0:
if time.monotonic() > deadline:
raise TimeoutError(f'nothing subscribed to {topic} after {timeout:.0f} s')
time.sleep(0.1)
# Subscribers to the other topics are created by the same nodes, in the same
# constructors; leave discovery a moment to catch up with them too.
time.sleep(2.0)
def wait_matched(name, publisher):
while publisher.get_subscription_count() < self.node.count_subscribers(name):
if time.monotonic() > deadline:
# Usually a subscriber asking for a QoS this publisher cannot offer.
subscribers = '\n'.join(
f' {info.node_namespace.rstrip("/")}/{info.node_name}: '
f'{info.qos_profile.reliability.name}, '
f'{info.qos_profile.durability.name}, '
f'{info.qos_profile.history.name} {info.qos_profile.depth}'
for info in self.node.get_subscriptions_info_by_topic(name))
raise TimeoutError(
f'{name} matched with {publisher.get_subscription_count()} of its '
f'{self.node.count_subscribers(name)} subscribers:\n{subscribers}')
time.sleep(0.1)
# Discovery may not have found every subscriber yet, and what is published before
# a subscriber is matched never reaches it: the first transforms of the bag, say,
# which the first frames need. Wait until the subscribers this node knows of, and
# those each publisher is matched with, have not changed for a while.
def counts():
return tuple((self.node.count_subscribers(name), publisher.get_subscription_count())
for name, publisher in list(self._publishers.values()) +
[('/tf', self._extra_tf)])
stable_since, last = time.monotonic(), counts()
while time.monotonic() - stable_since < 2.0:
if time.monotonic() > deadline:
raise TimeoutError('discovery did not settle')
time.sleep(0.1)
current = counts()
if current != last:
stable_since, last = time.monotonic(), current
for bag_topic, (name, publisher) in self._publishers.items():
if self.node.count_subscribers(name) > 0:
# TF too: the first frames' transforms would otherwise be lost, or not,
# depending on when discovery got there.
wait_matched(name, publisher)
if self._types[bag_topic] in GATED_TYPES:
self.gated.add(bag_topic)
wait_matched('/tf', self._extra_tf)
def play(self, progress: Optional[Callable[[int, int], None]] = None,
idle_timeout: float = 300.0) -> int:
"""Publish the whole bag(s); returns the number of gated messages published."""
pending = [] # (release time, sequence, topic, data, bag time)
sequence = 0
for release_ns, data in self._extra:
heapq.heappush(pending, (release_ns, sequence, None, data, release_ns))
sequence += 1
clock_ns = 0
published = 0
gated_total = self._count_gated()
def release(until_ns):
nonlocal clock_ns, published
while pending and pending[0][0] <= until_ns:
_, _, topic, data, t = heapq.heappop(pending)
if topic is None:
self._extra_tf.publish(data)
continue
_, publisher = self._publishers[topic]
if topic in self.gated:
# Also lets the TF listeners take in the TF released so far, before
# the frame needing it arrives: a node waiting for a transform
# sleeps, which would look idle.
self.tree.wait_idle(timeout=idle_timeout)
if t > clock_ns:
clock_ns = t
self._clock.publish(Clock(clock=_to_time(clock_ns)))
publisher.publish(data)
published += 1
if progress:
progress(published, gated_total)
else:
publisher.publish(data)
lookahead_ns = int(self.lookahead * 1e9)
for bag in self.bag_dirs:
reader = self._open(bag)
while reader.has_next():
topic, data, t = _read_next(reader)
# Sensor data is held back by the lookahead, so the TF of the next
# lookahead seconds goes out before it.
delay = lookahead_ns if topic in self.gated else 0
heapq.heappush(pending, (t + delay, sequence, topic, data, t))
sequence += 1
release(t)
release(float('inf'))
# A longer quiet than between two messages, for the last one. A node polling for
# its messages on a fast timer is never quiet that long (find_object_2d with
# Camera/4imageRate 0, as fast as possible: set a rate instead).
self.tree.wait_idle(idle_polls=50, timeout=idle_timeout)
return published
def play_chunked(self, done_stamp: Callable[[], float], chunk: float = 10.0,
slack: float = 2.0, stall_timeout: float = 30.0, rate: float = 1.0,
progress: Optional[Callable[[int, int], None]] = None) -> int:
"""Publish the whole bag(s) at their own pace (times `rate`), `chunk` seconds at a
time.
As `ros2 bag play`, but after each chunk, wait until the pipeline has caught up:
until done_stamp() (the stamp, in seconds, of the last output of its last node,
rtabmap's info for instance) is within `slack` seconds of the last sensor message
published. Waiting fails if done_stamp() stops moving for `stall_timeout` seconds,
as when a node crashed. Faster than play(), with no idle polling; but within a
chunk, a node slower than real time builds up a backlog in its queue, which must
be deep enough to hold it. Returns the number of gated messages published.
"""
pending = [] # (release time, sequence, topic, data, bag time)
sequence = 0
for release_ns, data in self._extra:
heapq.heappush(pending, (release_ns, sequence, None, data, release_ns))
sequence += 1
state = {'clock_ns': 0, 'published': 0, 'last_sensor_ns': 0, 'pace': None}
gated_total = self._count_gated()
chunk_ns = int(chunk * 1e9)
lookahead_ns = int(self.lookahead * 1e9)
def release(until_ns):
while pending and pending[0][0] <= until_ns:
release_ns, _, topic, data, t = heapq.heappop(pending)
# The bag's pace: from the wall time the chunk started at.
if state['pace'] is None:
state['pace'] = (time.monotonic(), release_ns)
wall, start_ns = state['pace']
delay = wall + (release_ns - start_ns) / 1e9 / rate - time.monotonic()
if delay > 0:
time.sleep(delay)
if topic is None:
self._extra_tf.publish(data)
continue
_, publisher = self._publishers[topic]
if topic in self.gated:
if t > state['clock_ns']:
state['clock_ns'] = t
self._clock.publish(Clock(clock=_to_time(t)))
publisher.publish(data)
state['last_sensor_ns'] = max(state['last_sensor_ns'], t)
state['published'] += 1
if progress:
progress(state['published'], gated_total)
else:
publisher.publish(data)
def wait_caught_up():
target = state['last_sensor_ns'] / 1e9 - slack
last, since = done_stamp(), time.monotonic()
while done_stamp() < target:
now, stamp = time.monotonic(), done_stamp()
if stamp != last:
last, since = stamp, now
elif now - since > stall_timeout:
raise TimeoutError(
f'the pipeline did not catch up: its last output is stamped '
f'{stamp:.3f}, {target - stamp:.1f} s short of the last sensor '
f'message published, and has not moved for {stall_timeout:.0f} s')
time.sleep(0.05)
state['pace'] = None # resume at the bag's pace from now
chunk_end = None
for bag in self.bag_dirs:
reader = self._open(bag)
while reader.has_next():
topic, data, t = _read_next(reader)
if chunk_end is None:
chunk_end = t + chunk_ns
elif t >= chunk_end:
release(chunk_end)
wait_caught_up()
chunk_end += chunk_ns
delay = lookahead_ns if topic in self.gated else 0
heapq.heappush(pending, (t + delay, sequence, topic, data, t))
sequence += 1
release(t)
release(float('inf'))
wait_caught_up()
return state['published']
def _count_gated(self) -> int:
return sum(t.message_count
for bag in self.bag_dirs
for t in self._open(bag).get_metadata().topics_with_message_count
if t.topic_metadata.name in self.gated)
def _to_time(ns: int):
from builtin_interfaces.msg import Time
return Time(sec=ns // 1000000000, nanosec=ns % 1000000000)