spatialdds-ros 0.4.0__tar.gz → 0.4.1__tar.gz

{spatialdds_ros-0.4.0/spatialdds_ros.egg-info → spatialdds_ros-0.4.1}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: spatialdds-ros
-Version: 0.4.0
+Version: 0.4.1
 Summary: A ROS 2 robot as a SpatialDDS participant
 Home-page: https://github.com/OpenArCloud/spatialdds-ros
 Author: Open AR Cloud

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1}/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://relaxng.org/ns/structure/1.0"?>
 <package format="3">
   <name>spatialdds_ros</name>
-  <version>0.4.0</version>
+  <version>0.4.1</version>
   <description>
     Makes a ROS 2 robot a SpatialDDS participant: its pose published as a
     spatial.owm entity on scene-scoped topics, goto commands consumed from

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1}/setup.py

@@ -42,7 +42,7 @@ LONG_DESCRIPTION = (HERE / "README.md").read_text(encoding="utf-8")
 
 setup(
     name="spatialdds-ros",
-    version="0.4.0",
+    version="0.4.1",
     # `spatialdds_idl18` ships alongside: the generated bindings annotate
     # nested members with their own fully-qualified names, so the package has
     # to be importable as `spatialdds_idl18` and cannot be nested inside this

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1}/spatialdds_ros/__init__.py

@@ -3,4 +3,4 @@
 See docs/extraction-census.md for what is configurable and why.
 """
 
-__version__ = "0.4.0"
+__version__ = "0.4.1"

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1}/spatialdds_ros/robot_node.py

@@ -198,6 +198,7 @@ def main(argv=None) -> int:                                   # pragma: no cover
                 from tf2_ros import Buffer, TransformListener
                 self._tf_buffer = Buffer()
                 self._tf_listener = TransformListener(self._tf_buffer, self)
+                self._tf_freshness = TfFreshness()
                 self._read_pose = self._pose_from_tf
             elif source == "amcl":
                 from geometry_msgs.msg import PoseWithCovarianceStamped
@@ -225,6 +226,14 @@ def main(argv=None) -> int:                                   # pragma: no cover
                 # not converged yet simply has no transform, and the adapter's
                 # own silence handling already describes that state correctly.
                 return
+            # A transform the buffer has already answered with is not a new
+            # observation. Asked for "the latest", the buffer keeps returning
+            # the last transform it heard after its publisher has died, and
+            # calling that a sighting every tick kept a dead robot ACTIVE and
+            # freshly stamped forever -- the floor republishing a ghost the
+            # adapter's silence rule could never catch (finding 10).
+            if not self._tf_freshness.new(tf.header.stamp):
+                return
             t = tf.transform.translation
             self.adapter.observed(t.x, t.y, _yaw_of(tf.transform.rotation))
 
@@ -378,6 +387,9 @@ def main(argv=None) -> int:                                   # pragma: no cover
             self._goal_type = NavigateToPose
             self._client = ActionClient(node, NavigateToPose, action_name)
             self._action_name = action_name
+            # Only the latest goal may report. A goal superseded by a newer
+            # one ends too, and its ending is not news about the robot.
+            self._seq = 0
 
         def available(self) -> Optional[str]:
             if not self._client.server_is_ready():
@@ -397,8 +409,48 @@ def main(argv=None) -> int:                                   # pragma: no cover
             goal.pose.pose.position.y = y
             goal.pose.pose.position.z = self._node.config.ground_z
             goal.pose.pose.orientation.w = 1.0
-            self._client.send_goal_async(goal)
-            return f"nav2 accepted a goal at ({x:.2f}, {y:.2f})"
+            self._seq += 1
+            seq = self._seq
+            future = self._client.send_goal_async(goal)
+            future.add_done_callback(
+                lambda f: self._on_response(f, seq, x, y))
+            # Sent, not accepted: nav2 has not answered yet. Until 0.4.1 this
+            # said "accepted", and whatever nav2 then decided went nowhere: a
+            # rejected or aborted goal reached the robot's log and no client
+            # (finding 11).
+            return f"sent a goal at ({x:.2f}, {y:.2f}) to nav2"
+
+        def _on_response(self, future, seq: int, x: float, y: float) -> None:
+            if seq != self._seq:
+                return
+            try:
+                handle = future.result()
+            except Exception as error:                        # noqa: BLE001
+                self._refuse(f"goal ({x:.2f}, {y:.2f}) could not be sent to "
+                             f"nav2: {error}")
+                return
+            if not handle.accepted:
+                self._refuse(goal_outcome(False, None, x, y))
+                return
+            handle.get_result_async().add_done_callback(
+                lambda f: self._on_result(f, seq, x, y))
+
+        def _on_result(self, future, seq: int, x: float, y: float) -> None:
+            if seq != self._seq:
+                return
+            try:
+                status = future.result().status
+            except Exception as error:                        # noqa: BLE001
+                self._refuse(f"goal ({x:.2f}, {y:.2f}): nav2 gave no result: "
+                             f"{error}")
+                return
+            why = goal_outcome(True, status, x, y)
+            if why:
+                self._refuse(why)
+
+        def _refuse(self, why: str) -> None:
+            self._node.adapter.refused(why)
+            self._node.get_logger().info(why)
 
     try:
         node = SpatialDdsRobot()
@@ -416,6 +468,49 @@ def main(argv=None) -> int:                                   # pragma: no cover
     return 0
 
 
+class TfFreshness:
+    """Whether a looked-up transform is a new observation, by its stamp.
+
+    Compared stamp to stamp, never to a clock, so a sim on its own time and a
+    robot on wall time are treated alike: a transform is new if its stamp is
+    not the one the buffer answered with last time.
+    """
+
+    def __init__(self) -> None:
+        self._last: Optional[tuple] = None
+
+    def new(self, stamp) -> bool:
+        key = (int(stamp.sec), int(stamp.nanosec))
+        if key == self._last:
+            return False
+        self._last = key
+        return True
+
+
+#: action_msgs/GoalStatus, the three endings.
+GOAL_SUCCEEDED, GOAL_CANCELED, GOAL_ABORTED = 4, 5, 6
+
+
+def goal_outcome(accepted: bool, status: Optional[int],
+                 x: float, y: float) -> Optional[str]:
+    """The refusal a goal's ending owes its requester, or None if it arrived.
+
+    A goal nav2 would not take, or gave up on, travels to whoever asked on the
+    robot's latched record (`RobotAdapter.refused`), as the kinematic fallback's
+    refusals always have.
+    """
+    if not accepted:
+        return f"goal ({x:.2f}, {y:.2f}) rejected by nav2"
+    if status == GOAL_SUCCEEDED:
+        return None
+    if status == GOAL_ABORTED:
+        return (f"goal ({x:.2f}, {y:.2f}) abandoned: nav2 could not reach it "
+                f"(no plan, or the controller gave up)")
+    if status == GOAL_CANCELED:
+        return f"goal ({x:.2f}, {y:.2f}) cancelled before it was reached"
+    return f"goal ({x:.2f}, {y:.2f}) ended in nav2 status {status}"
+
+
 def _yaw_of(q) -> float:
     """Yaw from a ROS quaternion message."""
     return math.atan2(2.0 * (q.w * q.z + q.x * q.y),

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1/spatialdds_ros.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: spatialdds-ros
-Version: 0.4.0
+Version: 0.4.1
 Summary: A ROS 2 robot as a SpatialDDS participant
 Home-page: https://github.com/OpenArCloud/spatialdds-ros
 Author: Open AR Cloud

{spatialdds_ros-0.4.0 → spatialdds_ros-0.4.1}/spatialdds_ros.egg-info/SOURCES.txt

@@ -101,6 +101,7 @@ test/test_frames.py
 test/test_generated_types.py
 test/test_kinematic.py
 test/test_no_ros_needed.py
+test/test_node_honesty.py
 test/test_packaging.py
 test/test_qos_and_tempo.py
 test/test_readme.py

spatialdds_ros-0.4.1/test/test_node_honesty.py

@@ -0,0 +1,45 @@
+"""The nav2 node says only what it saw: findings 10 and 11, without ROS.
+
+The node itself needs rclpy, so its two decisions are module-level and are
+checked here directly; the ROS run that shows them end to end is SpatialDDS-
+demo's `robot-captures.spec.ts`, "the sim dies...", against the nav2 tier.
+"""
+from types import SimpleNamespace
+
+from spatialdds_ros.robot_node import (
+    GOAL_ABORTED, GOAL_CANCELED, GOAL_SUCCEEDED, TfFreshness, goal_outcome)
+
+
+def stamp(sec, nanosec=0):
+    return SimpleNamespace(sec=sec, nanosec=nanosec)
+
+
+def test_a_transform_already_answered_is_not_a_new_observation():
+    """Finding 10: the buffer's last transform, asked for again after its
+    publisher died, must not count as a sighting."""
+    fresh = TfFreshness()
+    assert fresh.new(stamp(100, 5)) is True
+    assert fresh.new(stamp(100, 5)) is False      # the sim died here
+    assert fresh.new(stamp(100, 5)) is False
+    assert fresh.new(stamp(100, 6)) is True       # and came back
+
+
+def test_freshness_is_stamp_to_stamp_not_against_a_clock():
+    """A sim on its own time and a robot on wall time are treated alike."""
+    fresh = TfFreshness()
+    assert fresh.new(stamp(0, 1)) is True
+    assert fresh.new(stamp(1792950000, 0)) is True
+
+
+def test_a_goal_that_arrived_owes_no_refusal():
+    assert goal_outcome(True, GOAL_SUCCEEDED, 1, 2) is None
+
+
+def test_every_other_ending_is_a_refusal_naming_the_goal():
+    """Finding 11: rejected, aborted and cancelled goals reach the requester."""
+    for accepted, status, word in ((False, None, "rejected"),
+                                   (True, GOAL_ABORTED, "abandoned"),
+                                   (True, GOAL_CANCELED, "cancelled"),
+                                   (True, 2, "status 2")):
+        why = goal_outcome(accepted, status, 10.0, -12.0)
+        assert why and "(10.00, -12.00)" in why and word in why, why