From 8e5c54d02e9e076f41307fb0df8fac65100d204d Mon Sep 17 00:00:00 2001 From: ratheron Date: Mon, 19 Oct 2026 19:03:28 +0200 Subject: [PATCH] Rename the connector arrays to tf_* and est_* --- README.md | 2 +- docs/get-started/ros2.md | 8 +-- drone_estimators/ros/connector.py | 106 ++++++++++++++-------------- tests/integration/test_connector.py | 28 ++++---- 4 files changed, 72 insertions(+), 72 deletions(-) diff --git a/README.md b/README.md index 8829a36..a3f7434 100644 --- a/README.md +++ b/README.md @@ -117,6 +117,6 @@ from drone_estimators.ros import ROSConnector rclpy.init() conn = ROSConnector(estimator_names=["cf1"], timeout=10.0) -print(conn.pos["cf1"], conn.vel["cf1"]) +print(conn.est_pos["cf1"], conn.est_vel["cf1"]) conn.close() ``` diff --git a/docs/get-started/ros2.md b/docs/get-started/ros2.md index 048163a..6375b10 100644 --- a/docs/get-started/ros2.md +++ b/docs/get-started/ros2.md @@ -80,11 +80,11 @@ from drone_estimators.ros import ROSConnector if __name__ == "__main__": # The connector spawns processes, so guard the entry point rclpy.init() connector = ROSConnector(tf_names=["gate1", "cf1"], estimator_names=["cf1"], timeout=5.0) - gate_pos = connector.pos_raw["gate1"] # Raw pose of gate1 from /tf - cf1_raw_pos = connector.pos_raw["cf1"] # Raw pose of cf1 from /tf - pos, vel = connector.pos["cf1"], connector.vel["cf1"] # Estimated state of cf1 + gate_pos = connector.tf_pos["gate1"] # Raw pose of gate1 from /tf + cf1_raw_pos = connector.tf_pos["cf1"] # Raw pose of cf1 from /tf + pos, vel = connector.est_pos["cf1"], connector.est_vel["cf1"] # Estimated state of cf1 connector.close() rclpy.shutdown() ``` -Objects in `tf_names` provide their raw pose from `/tf` (`pos_raw`, `quat_raw`, `times_raw`), objects in `estimator_names` the full estimated state (`pos`, `quat`, `vel`, `ang_vel`, `rotor_vel`, `disturbance`, `times`). An object can be in both lists. Then, `is_outdated` is true if either its raw pose or its estimate is outdated. Since the estimator keeps publishing predictions when an object is occluded, add estimated objects to `tf_names` as well to detect lost tracking. With `timeout`, the connector waits until it has received data for all objects and raises a `TimeoutError` otherwise. The connector can also publish commands with `cmd_topic` and `publish_cmd`. +Objects in `tf_names` provide their raw pose from `/tf` (`tf_pos`, `tf_quat`, `tf_time`), objects in `estimator_names` the full estimated state (`est_pos`, `est_quat`, `est_vel`, `est_ang_vel`, `est_rotor_vel`, `est_disturbance`, `est_time`). An object can be in both lists. Then, `is_outdated` is true if either its raw pose or its estimate is outdated. Since the estimator keeps publishing predictions when an object is occluded, add estimated objects to `tf_names` as well to detect lost tracking. With `timeout`, the connector waits until it has received data for all objects and raises a `TimeoutError` otherwise. The connector can also publish commands with `cmd_topic` and `publish_cmd`. diff --git a/drone_estimators/ros/connector.py b/drone_estimators/ros/connector.py index 65152b4..3c3d207 100644 --- a/drone_estimators/ros/connector.py +++ b/drone_estimators/ros/connector.py @@ -47,9 +47,9 @@ class ROSConnector: The class supports two types of tracking: - * Raw pose tracking from `/tf` topics, available in `pos_raw` and `quat_raw` + * Raw pose tracking from `/tf` topics, available in `tf_pos` and `tf_quat` * Full state estimation including velocity, angular velocity, and disturbance forces from an - estimator node, available in `pos`, `quat`, `vel`, etc. + estimator node, available in `est_pos`, `est_quat`, `est_vel`, etc. An object can be tracked both ways at the same time. @@ -60,7 +60,7 @@ class ROSConnector: >>> # Track drone position and state from estimator >>> conn = ROSConnector(estimator_names=["cf1"], cmd_topic="/drones/cf1/cmd", timeout=1.0) >>> # Access the latest position data - >>> drone_pos = conn.pos["cf1"] + >>> drone_pos = conn.est_pos["cf1"] >>> # Publish a command (non-blocking) >>> conn.publish_cmd(np.array([1.0, 0.0, 0.0, 0.0])) >>> # Clean up when done @@ -109,16 +109,16 @@ def __init__( self._outdated_after = outdated_after # Create synchronized, shared arrays for all quantities ctx = mp.get_context("spawn") - self._pos_raw = ctx.Array("f", [float("nan")] * len(self.tf_names) * 3) - self._quat_raw = ctx.Array("f", [float("nan")] * len(self.tf_names) * 4) - self._times_raw = ctx.Array("d", [float("nan")] * len(self.tf_names)) - self._pos = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) - self._quat = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 4) - self._vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) - self._ang_vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) - self._disturbance = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 6) - self._rotor_vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 4) - self._times = ctx.Array("d", [float("nan")] * len(self.estimator_names)) + self._tf_pos = ctx.Array("f", [float("nan")] * len(self.tf_names) * 3) + self._tf_quat = ctx.Array("f", [float("nan")] * len(self.tf_names) * 4) + self._tf_time = ctx.Array("d", [float("nan")] * len(self.tf_names)) + self._est_pos = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) + self._est_quat = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 4) + self._est_vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) + self._est_ang_vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 3) + self._est_disturbance = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 6) + self._est_rotor_vel = ctx.Array("f", [float("nan")] * len(self.estimator_names) * 4) + self._est_time = ctx.Array("d", [float("nan")] * len(self.estimator_names)) # Note that times needs to be double, since time.time() might be a large number self.shutdown = ctx.Event() @@ -127,7 +127,7 @@ def __init__( if self.tf_names: # Create a process for the /tf callback tf_process = ctx.Process( target=tf_update, - args=(self.tf_names, self._pos_raw, self._quat_raw, self._times_raw, self.shutdown), + args=(self.tf_names, self._tf_pos, self._tf_quat, self._tf_time, self.shutdown), ) self.processes.append(tf_process) @@ -136,13 +136,13 @@ def __init__( target=estimate_update, args=( self.estimator_names, - self._pos, - self._quat, - self._vel, - self._ang_vel, - self._disturbance, - self._rotor_vel, - self._times, + self._est_pos, + self._est_quat, + self._est_vel, + self._est_ang_vel, + self._est_disturbance, + self._est_rotor_vel, + self._est_time, self.shutdown, ), ) @@ -169,8 +169,8 @@ def __init__( time.sleep(0.01) # Processes are spinning, so we can sleep here else: self.shutdown.set() - tf_objects = [n for n, pos in self.pos_raw.items() if np.any(np.isnan(pos))] - estimator_objects = [n for n, pos in self.pos.items() if np.any(np.isnan(pos))] + tf_objects = [n for n, pos in self.tf_pos.items() if np.any(np.isnan(pos))] + estimator_objects = [n for n, pos in self.est_pos.items() if np.any(np.isnan(pos))] missing_msg = "" if any(tf_objects): missing_msg += ( @@ -186,71 +186,71 @@ def __init__( ) @property - def pos_raw(self) -> dict[str, NDArray]: + def tf_pos(self) -> dict[str, NDArray]: """The latest raw position data from `/tf` for all objects in `tf_names`.""" - pos = np.asarray(self._pos_raw, dtype=np.float32).reshape(-1, 3) + pos = np.asarray(self._tf_pos, dtype=np.float32).reshape(-1, 3) return {n: pos[i] for i, n in enumerate(self.tf_names)} @property - def quat_raw(self) -> dict[str, NDArray]: + def tf_quat(self) -> dict[str, NDArray]: """The latest raw orientation data from `/tf` for all objects in `tf_names`.""" - quat = np.asarray(self._quat_raw, dtype=np.float32).reshape(-1, 4) + quat = np.asarray(self._tf_quat, dtype=np.float32).reshape(-1, 4) return {n: quat[i] for i, n in enumerate(self.tf_names)} @property - def times_raw(self) -> dict[str, np.float64]: + def tf_time(self) -> dict[str, np.float64]: """The times of the last received raw poses for all objects in `tf_names`.""" - times = np.asarray(self._times_raw, dtype=np.float64).reshape(-1) + times = np.asarray(self._tf_time, dtype=np.float64).reshape(-1) return {n: times[i] for i, n in enumerate(self.tf_names)} @property - def pos(self) -> dict[str, NDArray]: + def est_pos(self) -> dict[str, NDArray]: """The latest estimated position data for all objects in `estimator_names`.""" - pos = np.asarray(self._pos, dtype=np.float32).reshape(-1, 3) + pos = np.asarray(self._est_pos, dtype=np.float32).reshape(-1, 3) return {n: pos[i] for i, n in enumerate(self.estimator_names)} @property - def quat(self) -> dict[str, NDArray]: + def est_quat(self) -> dict[str, NDArray]: """The latest estimated orientation data for all objects in `estimator_names`.""" - quat = np.asarray(self._quat, dtype=np.float32).reshape(-1, 4) + quat = np.asarray(self._est_quat, dtype=np.float32).reshape(-1, 4) return {n: quat[i] for i, n in enumerate(self.estimator_names)} @property - def vel(self) -> dict[str, NDArray]: + def est_vel(self) -> dict[str, NDArray]: """The latest velocity data for all tracked objects.""" - vel = np.asarray(self._vel, dtype=np.float32).reshape(-1, 3) + vel = np.asarray(self._est_vel, dtype=np.float32).reshape(-1, 3) return {n: vel[i] for i, n in enumerate(self.estimator_names)} @property - def ang_vel(self) -> dict[str, NDArray]: + def est_ang_vel(self) -> dict[str, NDArray]: """The latest angular velocity data for all tracked objects.""" - ang_vel = np.asarray(self._ang_vel, dtype=np.float32).reshape(-1, 3) + ang_vel = np.asarray(self._est_ang_vel, dtype=np.float32).reshape(-1, 3) return {n: ang_vel[i] for i, n in enumerate(self.estimator_names)} @property - def disturbance(self) -> dict[str, NDArray]: + def est_disturbance(self) -> dict[str, NDArray]: """The latest disturbance data for all tracked objects.""" - disturbance = np.asarray(self._disturbance, dtype=np.float32).reshape(-1, 6) + disturbance = np.asarray(self._est_disturbance, dtype=np.float32).reshape(-1, 6) return {n: disturbance[i] for i, n in enumerate(self.estimator_names)} @property - def rotor_vel(self) -> dict[str, NDArray]: + def est_rotor_vel(self) -> dict[str, NDArray]: """The latest estimated rotor velocities for all objects in `estimator_names`.""" - rotor_vel = np.asarray(self._rotor_vel, dtype=np.float32).reshape(-1, 4) + rotor_vel = np.asarray(self._est_rotor_vel, dtype=np.float32).reshape(-1, 4) return {n: rotor_vel[i] for i, n in enumerate(self.estimator_names)} @property - def times(self) -> dict[str, np.float64]: + def est_time(self) -> dict[str, np.float64]: """The times of the last received estimates for all objects in `estimator_names`.""" - times = np.asarray(self._times, dtype=np.float64).reshape(-1) + times = np.asarray(self._est_time, dtype=np.float64).reshape(-1) return {n: times[i] for i, n in enumerate(self.estimator_names)} @property def is_outdated(self) -> dict[str, np.bool]: """Whether the raw pose or the estimate of each tracked object is outdated.""" t_outdated = time.time() - self._outdated_after - outdated = {n: t < t_outdated for n, t in self.times_raw.items()} - for n, t in self.times.items(): + outdated = {n: t < t_outdated for n, t in self.tf_time.items()} + for n, t in self.est_time.items(): outdated[n] = outdated.get(n, False) | (t < t_outdated) return outdated @@ -258,14 +258,14 @@ def is_outdated(self) -> dict[str, np.bool]: def active(self) -> bool: """Check if each object has received an update already.""" # Check if drone is being tracked and if drone has already received updates - pos_raw_ready = not np.any(np.isnan(np.array(self._pos_raw, dtype=np.float32))) - quat_raw_ready = not np.any(np.isnan(np.array(self._quat_raw, dtype=np.float32))) - pos_ready = not np.any(np.isnan(np.array(self._pos, dtype=np.float32))) - quat_ready = not np.any(np.isnan(np.array(self._quat, dtype=np.float32))) - vel_ready = not np.any(np.isnan(np.array(self._vel, dtype=np.float32))) - ang_vel_ready = not np.any(np.isnan(np.array(self._ang_vel, dtype=np.float32))) - raw_ready = pos_raw_ready and quat_raw_ready - return raw_ready and pos_ready and quat_ready and vel_ready and ang_vel_ready + tf_pos_ready = not np.any(np.isnan(np.array(self._tf_pos, dtype=np.float32))) + tf_quat_ready = not np.any(np.isnan(np.array(self._tf_quat, dtype=np.float32))) + est_pos_ready = not np.any(np.isnan(np.array(self._est_pos, dtype=np.float32))) + est_quat_ready = not np.any(np.isnan(np.array(self._est_quat, dtype=np.float32))) + est_vel_ready = not np.any(np.isnan(np.array(self._est_vel, dtype=np.float32))) + est_ang_vel_ready = not np.any(np.isnan(np.array(self._est_ang_vel, dtype=np.float32))) + tf_ready = tf_pos_ready and tf_quat_ready + return tf_ready and est_pos_ready and est_quat_ready and est_vel_ready and est_ang_vel_ready def publish_cmd(self, value: NDArray): """Publish a command to ROS. diff --git a/tests/integration/test_connector.py b/tests/integration/test_connector.py index fb9e852..b48bb29 100644 --- a/tests/integration/test_connector.py +++ b/tests/integration/test_connector.py @@ -98,10 +98,10 @@ def connect(**kwargs: list[str]) -> ROSConnector: def test_tf_names(fake: SimpleNamespace, connect: Callable[..., ROSConnector]): """Tests that objects in tf_names provide their raw poses.""" connector = connect(tf_names=[fake.gate]) - assert np.allclose(connector.pos_raw[fake.gate], GATE_POS) - assert np.allclose(connector.quat_raw[fake.gate], GATE_QUAT) - assert not np.isnan(connector.times_raw[fake.gate]) - assert fake.gate not in connector.pos + assert np.allclose(connector.tf_pos[fake.gate], GATE_POS) + assert np.allclose(connector.tf_quat[fake.gate], GATE_QUAT) + assert not np.isnan(connector.tf_time[fake.gate]) + assert fake.gate not in connector.est_pos @pytest.mark.integration @@ -109,22 +109,22 @@ def test_estimator_names(fake: SimpleNamespace, connect: Callable[..., ROSConnec """Tests that objects in estimator_names provide their estimates.""" connector = connect(estimator_names=[fake.drone]) time.sleep(0.2) # The timeout does not wait for rotor velocities - assert np.allclose(connector.pos[fake.drone], EST_POS) - assert np.allclose(connector.quat[fake.drone], EST_QUAT) - assert np.allclose(connector.vel[fake.drone], EST_VEL) - assert np.allclose(connector.rotor_vel[fake.drone], EST_ROTOR_VEL) - assert not np.isnan(connector.times[fake.drone]) - assert fake.drone not in connector.pos_raw + assert np.allclose(connector.est_pos[fake.drone], EST_POS) + assert np.allclose(connector.est_quat[fake.drone], EST_QUAT) + assert np.allclose(connector.est_vel[fake.drone], EST_VEL) + assert np.allclose(connector.est_rotor_vel[fake.drone], EST_ROTOR_VEL) + assert not np.isnan(connector.est_time[fake.drone]) + assert fake.drone not in connector.tf_pos @pytest.mark.integration def test_tf_and_estimator_names(fake: SimpleNamespace, connect: Callable[..., ROSConnector]): """Tests that objects can provide their raw poses and estimates at the same time.""" connector = connect(tf_names=[fake.drone], estimator_names=[fake.drone]) - assert np.allclose(connector.pos_raw[fake.drone], RAW_POS) - assert np.allclose(connector.quat_raw[fake.drone], RAW_QUAT) - assert np.allclose(connector.pos[fake.drone], EST_POS) - assert np.allclose(connector.quat[fake.drone], EST_QUAT) + assert np.allclose(connector.tf_pos[fake.drone], RAW_POS) + assert np.allclose(connector.tf_quat[fake.drone], RAW_QUAT) + assert np.allclose(connector.est_pos[fake.drone], EST_POS) + assert np.allclose(connector.est_quat[fake.drone], EST_QUAT) @pytest.mark.integration