From bcccc398fdd35291eeb7229640e4e3b123142be2 Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Thu, 2 Jul 2026 18:38:58 +0900 Subject: [PATCH 1/5] Add initial robot_overview.py --- scripts/robot_overview.py | 303 ++++++++++++++++++ .../config/sim_game_settings_12.yaml | 4 +- .../config/sim_game_settings_14.yaml | 2 +- .../bitbots_mujoco_sim/simulation.py | 25 ++ 4 files changed, 331 insertions(+), 3 deletions(-) create mode 100644 scripts/robot_overview.py diff --git a/scripts/robot_overview.py b/scripts/robot_overview.py new file mode 100644 index 0000000000..009b8417fb --- /dev/null +++ b/scripts/robot_overview.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python3 +"""Overview of robot states across multiple ROS domains in the MuJoCo multi-robot sim. + +Each robot's teamplayer stack runs in its own ROS_DOMAIN_ID (11, 12, 13, ... see +``mujoco_simulation.launch.py``). A single ROS process normally only sees one domain, +so this script creates one independent ``rclpy.Context`` per domain, each initialized +with its own ``domain_id`` and spun in its own thread. The main thread periodically +prints a refreshing overview table. + +Self-contained: source the workspace (for the message types) and run e.g.:: + + python3 scripts/robot_overview.py --domains 11 12 13 14 + python3 scripts/robot_overview.py --num-robots 4 # -> domains 11..14 + python3 scripts/robot_overview.py --once # single snapshot, then exit + +Per-robot state topics are read at the domain root (the ``robotN/`` namespace only +exists in the main domain; inside a robot's own domain everything is at ``/``). The +ground-truth pose is the exception: the sim publishes ``robotN/true_pose`` only in the +main domain (never bridged into a robot's domain), so it is read there and compared +against each robot's localized ``pose_with_covariance`` to show the localization error. +""" + +import argparse +import math +import os +import threading +import time +from dataclasses import dataclass, field +from typing import Optional + +import rclpy +import transforms3d +from game_controller_hsl_interfaces.msg import GameState +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from std_msgs.msg import Float32 + +from bitbots_msgs.msg import Strategy + +# ---- Enum -> label maps (kept in sync with the .msg definitions) ------------------ + +ROLE = { + Strategy.ROLE_UNDEFINED: "undef", + Strategy.ROLE_IDLING: "idling", + Strategy.ROLE_OTHER: "other", + Strategy.ROLE_STRIKER: "striker", + Strategy.ROLE_SUPPORTER: "support", + Strategy.ROLE_DEFENDER: "defender", + Strategy.ROLE_GOALIE: "goalie", +} + +ACTION = { + Strategy.ACTION_UNDEFINED: "undef", + Strategy.ACTION_POSITIONING: "positioning", + Strategy.ACTION_GOING_TO_BALL: "going_to_ball", + Strategy.ACTION_TRYING_TO_SCORE: "trying_to_score", + Strategy.ACTION_WAITING: "waiting", + Strategy.ACTION_KICKING: "kicking", + Strategy.ACTION_SEARCHING: "searching", + Strategy.ACTION_LOCALIZING: "localizing", + Strategy.ACTION_PASSIVE: "passive", +} + +GAME_STATE = { + GameState.STATE_INITIAL: "INITIAL", + GameState.STATE_READY: "READY", + GameState.STATE_SET: "SET", + GameState.STATE_PLAYING: "PLAYING", + GameState.STATE_FINISHED: "FINISHED", +} + + +@dataclass +class Stamped: + """A latest-value slot with the wall-clock time it arrived (for staleness).""" + + value: object = None + at: float = 0.0 + + def set(self, value): + self.value = value + self.at = time.monotonic() + + def age(self) -> Optional[float]: + return None if self.value is None else time.monotonic() - self.at + + +@dataclass +class RobotState: + """Latest cached messages for one robot domain. Callbacks only write here.""" + + domain: int + strategy: Stamped = field(default_factory=Stamped) + pose: Stamped = field(default_factory=Stamped) # localization estimate (robot domain) + gamestate: Stamped = field(default_factory=Stamped) + time_to_ball: Stamped = field(default_factory=Stamped) + cmd_vel: Stamped = field(default_factory=Stamped) + true_pose: Stamped = field(default_factory=Stamped) # ground truth (main domain) + + +class _MonitorBase: + """A rclpy context + node + executor bound to a single ROS domain, spun in a thread.""" + + def __init__(self, domain: int, node_name: str): + self.context = Context() + self.context.init(domain_id=domain) + self.node = Node(node_name, context=self.context) + self.executor = SingleThreadedExecutor(context=self.context) + self.executor.add_node(self.node) + self._thread = threading.Thread(target=self._spin, daemon=True) + + @staticmethod + def _store(slot: Stamped): + return lambda msg: slot.set(msg) + + def _spin(self): + try: + self.executor.spin() + except Exception: # noqa: BLE001 - context shutdown races on exit are expected + pass + + def start(self): + self._thread.start() + + def shutdown(self): + self.executor.shutdown() + self.node.destroy_node() + self.context.try_shutdown() + + +class DomainMonitor(_MonitorBase): + """Reads one robot's own-domain state topics into the given shared RobotState.""" + + def __init__(self, state: RobotState): + super().__init__(state.domain, f"robot_overview_probe_{state.domain}") + self.state = state + self.node.create_subscription(Strategy, "strategy", self._store(state.strategy), 1) + self.node.create_subscription(PoseWithCovarianceStamped, "pose_with_covariance", self._store(state.pose), 1) + self.node.create_subscription(GameState, "gamestate", self._store(state.gamestate), 1) + self.node.create_subscription(Float32, "time_to_ball", self._store(state.time_to_ball), 1) + self.node.create_subscription(Twist, "cmd_vel", self._store(state.cmd_vel), 1) + + +class GroundTruthMonitor(_MonitorBase): + """Reads every robot's ``robotN/true_pose`` from the main domain (never bridged).""" + + def __init__(self, states: list[RobotState], main_domain: int): + super().__init__(main_domain, "robot_overview_truth") + for state in states: + self.node.create_subscription( + PoseStamped, f"robot{state.domain}/true_pose", self._store(state.true_pose), 1 + ) + + +# ---- Rendering -------------------------------------------------------------------- + + +def _xy_yaw(msg) -> Optional[tuple[float, float, float]]: + """Extract (x, y, yaw) from a PoseStamped or PoseWithCovarianceStamped, in radians.""" + if msg is None: + return None + pose = msg.pose.pose if isinstance(msg, PoseWithCovarianceStamped) else msg.pose + q = pose.orientation + yaw = transforms3d.euler.quat2euler([q.w, q.x, q.y, q.z])[2] + return (pose.position.x, pose.position.y, yaw) + + +def _rotate_180_about_origin(pose: Optional[tuple[float, float, float]]) -> Optional[tuple[float, float, float]]: + """Rotate an (x, y, yaw) pose 180° about the field origin. + + The kid-size field is point-symmetric about its center, so a 180° rotation + (x, y, yaw) -> (-x, -y, yaw + 180°) maps a pose onto a physically equivalent one. + We use it to bring the sim's ground-truth (MuJoCo world) frame into the same + orientation as the localization ``map`` frame, which is rotated 180° relative to it. + """ + if pose is None: + return None + x, y, yaw = pose + return (-x, -y, (yaw + 2 * math.pi) % (2 * math.pi) - math.pi) # == wrap(yaw + 180°) + + +def _fmt_pose(pose: Optional[tuple[float, float, float]]) -> str: + if pose is None: + return " - " + x, y, yaw = pose + return f"({x:+5.2f},{y:+5.2f}) {math.degrees(yaw):+4.0f}°" + + +def _fmt_error(est: Optional[tuple[float, float, float]], true: Optional[tuple[float, float, float]]) -> str: + if est is None or true is None: + return " - " + dist = math.hypot(est[0] - true[0], est[1] - true[1]) + # wrap yaw error into (-180, 180] + yaw_err = math.degrees((est[2] - true[2] + math.pi) % (2 * math.pi) - math.pi) + return f"{dist:4.2f}m {yaw_err:+4.0f}°" + + +def _fmt_age(age: Optional[float]) -> str: + if age is None: + return " - " + if age > 5.0: + return f"{age:4.0f}s" # clearly stale + return f"{age:4.1f}s" + + +def render(states: list[RobotState], rotate_180: bool = True) -> str: + header = ( + f"{'robot':>6} | {'role':>8} | {'action':>15} | {'est (x,y) yaw':^17} | " + f"{'true (x,y) yaw':^17} | {'err (m,°)':^11} | {'ttb':>6} | {'game':>8} | {'pen':>3} | {'age':>5}" + ) + lines = [header, "-" * len(header)] + for s in states: + strat: Optional[Strategy] = s.strategy.value + gs: Optional[GameState] = s.gamestate.value + ttb: Optional[Float32] = s.time_to_ball.value + est = _xy_yaw(s.pose.value) + true = _xy_yaw(s.true_pose.value) + if rotate_180: + true = _rotate_180_about_origin(true) + + role = ROLE.get(strat.role, "?") if strat else "-" + action = ACTION.get(strat.action, "?") if strat else "-" + ttb_str = f"{ttb.data:5.1f}s" if ttb else " - " + game = GAME_STATE.get(gs.main_state, "?") if gs else "-" + pen = ("Y" if gs.penalized else "n") if gs and hasattr(gs, "penalized") else "-" + # freshest signal we have from this robot -> its liveness + ages = [a for a in (s.strategy.age(), s.pose.age(), s.gamestate.age()) if a is not None] + age = _fmt_age(min(ages) if ages else None) + + lines.append( + f"{s.domain:>6} | {role:>8} | {action:>15} | {_fmt_pose(est):^17} | " + f"{_fmt_pose(true):^17} | {_fmt_error(est, true):^11} | " + f"{ttb_str:>6} | {game:>8} | {pen:>3} | {age:>5}" + ) + return "\n".join(lines) + + +def parse_args(): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + g = ap.add_mutually_exclusive_group() + g.add_argument("--domains", type=int, nargs="+", help="Explicit ROS_DOMAIN_IDs to monitor, e.g. 11 12 13 14") + g.add_argument("--num-robots", type=int, help="Monitor domains 11 .. 11+N-1") + ap.add_argument("--rate", type=float, default=2.0, help="Refresh rate in Hz (default: 2)") + ap.add_argument( + "--main-domain", + type=int, + default=int(os.getenv("ROS_DOMAIN_ID", "0")), + help="Domain the sim runs in, where robotN/true_pose is published (default: $ROS_DOMAIN_ID or 0)", + ) + ap.add_argument("--once", action="store_true", help="Collect briefly, print one snapshot, then exit") + ap.add_argument( + "--rotate-180", + action=argparse.BooleanOptionalAction, + default=True, + help="Rotate the ground-truth pose 180° about the origin to match the localization " + "map frame (the field is symmetric, so this only realigns the frames). Use " + "--no-rotate-180 if your sim world and map frame are already aligned.", + ) + return ap.parse_args() + + +def main(): + args = parse_args() + if args.domains: + domains = args.domains + elif args.num_robots: + domains = list(range(11, 11 + args.num_robots)) + else: + domains = [11, 12, 13, 14] + + states = [RobotState(domain=d) for d in domains] + monitors: list[_MonitorBase] = [DomainMonitor(s) for s in states] + monitors.append(GroundTruthMonitor(states, args.main_domain)) + for m in monitors: + m.start() + + try: + if args.once: + time.sleep(1.0) # let subscriptions match and receive + print(render(states, args.rotate_180)) + else: + period = 1.0 / args.rate + while True: + # clear screen + home cursor, then redraw the table in place + print("\033[2J\033[H", end="") + rot = " (truth rotated 180°)" if args.rotate_180 else "" + print( + f"Robot overview domains={domains} truth@{args.main_domain}{rot} " + f"({time.strftime('%H:%M:%S')})\n" + ) + print(render(states, args.rotate_180)) + time.sleep(period) + except KeyboardInterrupt: + pass + finally: + for m in monitors: + m.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_12.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_12.yaml index 437d64089b..3868035713 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_12.yaml +++ b/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_12.yaml @@ -1,5 +1,5 @@ parameter_blackboard: ros__parameters: bot_id: 2 - position_number: 0 - role: goalie + position_number: 1 + role: offense diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_14.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_14.yaml index 7116cb8607..d4889513bd 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_14.yaml +++ b/src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_14.yaml @@ -2,4 +2,4 @@ parameter_blackboard: ros__parameters: bot_id: 4 position_number: 1 - role: defense + role: goalie diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py index 5c6f5ed025..fbcde8d515 100644 --- a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py +++ b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py @@ -5,6 +5,7 @@ import mujoco import numpy as np from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import PoseStamped from mujoco import viewer from rclpy.node import Node from rclpy.time import Time @@ -106,6 +107,7 @@ def __init__(self): {"frequency": 1, "handler": self.publish_clock_event}, {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_ros_joint_states_event())}, {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_imu_event())}, + {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_true_pose_event())}, {"frequency": 32, "handler": lambda: self.publish(lambda robot: robot.publish_camera_event())}, ] @@ -383,6 +385,10 @@ def _topic(name: str) -> str: "imu": self.simulation.create_publisher(Imu, _topic("imu/data"), 1), "camera_proc": self.simulation.create_publisher(Image, _topic("zed/zed_node/rgb/image_rect_color"), 1), "camera_info": self.simulation.create_publisher(CameraInfo, _topic("zed/zed_node/rgb/camera_info"), 1), + # Ground-truth base_link pose, published only in the sim's (base) domain and + # intentionally NOT added to the domain bridge, so a robot's own stack cannot + # see it. Used by scripts/robot_overview.py to compute the localization error. + "true_pose": self.simulation.create_publisher(PoseStamped, _topic("true_pose"), 1), } self.simulation.create_subscription(JointCommand, _topic("joint_command"), self.joint_command_callback, 1) @@ -439,6 +445,25 @@ def publish_imu_event(self) -> None: ) self.node_publishers["imu"].publish(imu) + def publish_true_pose_event(self) -> None: + """Publish the noise-free base_link pose straight from the MuJoCo state. + + MuJoCo's world frame is the field-centered frame, which matches the ``map`` frame + the localization estimate is expressed in, so the two are directly comparable. + """ + pose = PoseStamped() + pose.header.stamp = self.simulation.time_message + pose.header.frame_id = "map" + + position = self.data.xpos[self.robot.base_body_id] + orientation = self.data.xquat[self.robot.base_body_id] # MuJoCo order: (w, x, y, z) + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (float(v) for v in position) + pose.pose.orientation.w = float(orientation[0]) + pose.pose.orientation.x = float(orientation[1]) + pose.pose.orientation.y = float(orientation[2]) + pose.pose.orientation.z = float(orientation[3]) + self.node_publishers["true_pose"].publish(pose) + def publish_camera_event(self) -> None: if not self.simulation.camera_active: return From 683b128bbfe18adbceff2c716d32964a8efce438 Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Fri, 3 Jul 2026 17:12:02 +0900 Subject: [PATCH 2/5] Add ball position error tracking --- scripts/robot_overview.py | 81 ++++++++++++++----- .../config/mickey.yaml | 2 +- .../bitbots_mujoco_sim/simulation.py | 20 ++++- 3 files changed, 82 insertions(+), 21 deletions(-) diff --git a/scripts/robot_overview.py b/scripts/robot_overview.py index 009b8417fb..4a0ed6e326 100644 --- a/scripts/robot_overview.py +++ b/scripts/robot_overview.py @@ -29,13 +29,17 @@ from typing import Optional import rclpy +import tf2_geometry_msgs # noqa: F401 -- registers the PointStamped transform used below +import tf2_ros import transforms3d from game_controller_hsl_interfaces.msg import GameState -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist from rclpy.context import Context +from rclpy.duration import Duration from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node -from std_msgs.msg import Float32 +from rclpy.time import Time +from std_msgs.msg import Float32, Header from bitbots_msgs.msg import Strategy @@ -98,6 +102,7 @@ class RobotState: time_to_ball: Stamped = field(default_factory=Stamped) cmd_vel: Stamped = field(default_factory=Stamped) true_pose: Stamped = field(default_factory=Stamped) # ground truth (main domain) + ball_map: Stamped = field(default_factory=Stamped) # filtered ball, transformed to map by us class _MonitorBase: @@ -133,21 +138,45 @@ def shutdown(self): class DomainMonitor(_MonitorBase): """Reads one robot's own-domain state topics into the given shared RobotState.""" - def __init__(self, state: RobotState): + def __init__(self, state: RobotState, map_frame: str = "map"): super().__init__(state.domain, f"robot_overview_probe_{state.domain}") self.state = state + self.map_frame = map_frame self.node.create_subscription(Strategy, "strategy", self._store(state.strategy), 1) self.node.create_subscription(PoseWithCovarianceStamped, "pose_with_covariance", self._store(state.pose), 1) self.node.create_subscription(GameState, "gamestate", self._store(state.gamestate), 1) self.node.create_subscription(Float32, "time_to_ball", self._store(state.time_to_ball), 1) self.node.create_subscription(Twist, "cmd_vel", self._store(state.cmd_vel), 1) + # The filtered ball is published in the odom frame; we hold this domain's TF tree + # ourselves and transform each ball into the map frame so it is comparable to the + # ground-truth ball. The listener feeds off this domain's /tf and /tf_static. + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + self.node.create_subscription( + PoseWithCovarianceStamped, "ball_position_relative_filtered", self._ball_filtered_cb, 1 + ) + + def _ball_filtered_cb(self, msg: PoseWithCovarianceStamped) -> None: + # Stamp 0 -> use the latest available transform (avoids sim-time extrapolation issues). + point = PointStamped( + header=Header(stamp=Time().to_msg(), frame_id=msg.header.frame_id), + point=msg.pose.pose.position, + ) + try: + self.state.ball_map.set(self.tf_buffer.transform(point, self.map_frame, timeout=Duration())) + except tf2_ros.TransformException: + pass # TF not ready yet / frame missing — keep the previous value + class GroundTruthMonitor(_MonitorBase): - """Reads every robot's ``robotN/true_pose`` from the main domain (never bridged).""" + """Reads the sim's ground truth from the main domain (never bridged): per-robot + ``robotN/true_pose`` and the single global ``true_ball``.""" def __init__(self, states: list[RobotState], main_domain: int): super().__init__(main_domain, "robot_overview_truth") + self.true_ball = Stamped() # single global ball, shared across robots + self.node.create_subscription(PointStamped, "true_ball", self._store(self.true_ball), 1) for state in states: self.node.create_subscription( PoseStamped, f"robot{state.domain}/true_pose", self._store(state.true_pose), 1 @@ -181,13 +210,6 @@ def _rotate_180_about_origin(pose: Optional[tuple[float, float, float]]) -> Opti return (-x, -y, (yaw + 2 * math.pi) % (2 * math.pi) - math.pi) # == wrap(yaw + 180°) -def _fmt_pose(pose: Optional[tuple[float, float, float]]) -> str: - if pose is None: - return " - " - x, y, yaw = pose - return f"({x:+5.2f},{y:+5.2f}) {math.degrees(yaw):+4.0f}°" - - def _fmt_error(est: Optional[tuple[float, float, float]], true: Optional[tuple[float, float, float]]) -> str: if est is None or true is None: return " - " @@ -197,6 +219,16 @@ def _fmt_error(est: Optional[tuple[float, float, float]], true: Optional[tuple[f return f"{dist:4.2f}m {yaw_err:+4.0f}°" +def _ball_xy(msg: Optional[PointStamped]) -> Optional[tuple[float, float]]: + return None if msg is None else (msg.point.x, msg.point.y) + + +def _fmt_dist(a: Optional[tuple[float, float]], b: Optional[tuple[float, float]]) -> str: + if a is None or b is None: + return " - " + return f"{math.hypot(a[0] - b[0], a[1] - b[1]):5.2f}m" + + def _fmt_age(age: Optional[float]) -> str: if age is None: return " - " @@ -205,10 +237,12 @@ def _fmt_age(age: Optional[float]) -> str: return f"{age:4.1f}s" -def render(states: list[RobotState], rotate_180: bool = True) -> str: +def render(states: list[RobotState], true_ball: Optional[tuple[float, float]] = None, rotate_180: bool = True) -> str: + # true_ball is the sim ground-truth ball (map frame). ball_err is the distance between + # the filtered ball (transformed to map) and the ground-truth ball (position only). header = ( - f"{'robot':>6} | {'role':>8} | {'action':>15} | {'est (x,y) yaw':^17} | " - f"{'true (x,y) yaw':^17} | {'err (m,°)':^11} | {'ttb':>6} | {'game':>8} | {'pen':>3} | {'age':>5}" + f"{'robot':>6} | {'role':>8} | {'action':>15} | {'pose err (m,°)':^14} | " + f"{'ball err':>8} | {'ttb':>6} | {'game':>8} | {'pen':>3} | {'age':>5}" ) lines = [header, "-" * len(header)] for s in states: @@ -222,6 +256,7 @@ def render(states: list[RobotState], rotate_180: bool = True) -> str: role = ROLE.get(strat.role, "?") if strat else "-" action = ACTION.get(strat.action, "?") if strat else "-" + ball_err = _fmt_dist(_ball_xy(s.ball_map.value), true_ball) ttb_str = f"{ttb.data:5.1f}s" if ttb else " - " game = GAME_STATE.get(gs.main_state, "?") if gs else "-" pen = ("Y" if gs.penalized else "n") if gs and hasattr(gs, "penalized") else "-" @@ -230,8 +265,8 @@ def render(states: list[RobotState], rotate_180: bool = True) -> str: age = _fmt_age(min(ages) if ages else None) lines.append( - f"{s.domain:>6} | {role:>8} | {action:>15} | {_fmt_pose(est):^17} | " - f"{_fmt_pose(true):^17} | {_fmt_error(est, true):^11} | " + f"{s.domain:>6} | {role:>8} | {action:>15} | {_fmt_error(est, true):^14} | " + f"{ball_err:>8} | " f"{ttb_str:>6} | {game:>8} | {pen:>3} | {age:>5}" ) return "\n".join(lines) @@ -271,15 +306,22 @@ def main(): domains = [11, 12, 13, 14] states = [RobotState(domain=d) for d in domains] + truth = GroundTruthMonitor(states, args.main_domain) monitors: list[_MonitorBase] = [DomainMonitor(s) for s in states] - monitors.append(GroundTruthMonitor(states, args.main_domain)) + monitors.append(truth) for m in monitors: m.start() + def true_ball_xy() -> Optional[tuple[float, float]]: + xy = _ball_xy(truth.true_ball.value) + if xy is None or not args.rotate_180: + return xy + return (-xy[0], -xy[1]) # same 180° realignment as the ground-truth pose + try: if args.once: time.sleep(1.0) # let subscriptions match and receive - print(render(states, args.rotate_180)) + print(render(states, true_ball_xy(), args.rotate_180)) else: period = 1.0 / args.rate while True: @@ -289,8 +331,9 @@ def main(): print( f"Robot overview domains={domains} truth@{args.main_domain}{rot} " f"({time.strftime('%H:%M:%S')})\n" + "ball err = filtered ball (transformed odom->map) vs true ball\n" ) - print(render(states, args.rotate_180)) + print(render(states, true_ball_xy(), args.rotate_180)) time.sleep(period) except KeyboardInterrupt: pass diff --git a/src/bitbots_misc/bitbots_extrinsic_calibration/config/mickey.yaml b/src/bitbots_misc/bitbots_extrinsic_calibration/config/mickey.yaml index 1a8a2c4011..93bdfd39d1 100644 --- a/src/bitbots_misc/bitbots_extrinsic_calibration/config/mickey.yaml +++ b/src/bitbots_misc/bitbots_extrinsic_calibration/config/mickey.yaml @@ -1,6 +1,6 @@ /bitbots_extrinsic_camera_calibration: ros__parameters: - offset_x: 0.43 + offset_x: 0.0 offset_y: 0.0 offset_z: 0.0 diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py index 999fb8a6a0..bb6aabaf06 100644 --- a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py +++ b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py @@ -5,7 +5,7 @@ import mujoco import numpy as np from ament_index_python.packages import get_package_share_directory -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PointStamped, PoseStamped from mujoco import viewer from rclpy.node import Node from rclpy.time import Time @@ -101,6 +101,13 @@ def __init__(self): self.clock_publisher = self.create_publisher(Clock, "clock", 1) self.create_subscription(Float32, "real_time_factor", self.real_time_factor_callback, 1) + # Ground-truth ball position, published only in the sim's (base) domain and, like + # true_pose, intentionally NOT bridged into any robot domain. Used by + # scripts/robot_overview.py to compare against the behavior's ball estimate. + body_names = {self.model.body(i).name for i in range(self.model.nbody)} + self.ball_body_id = self.model.body("ball").id if "ball" in body_names else None + self.true_ball_publisher = self.create_publisher(PointStamped, "true_ball", 1) + self.imu_frame_id = self.get_parameter("imu_frame").get_parameter_value().string_value self.camera_optical_frame_id = self.get_parameter("camera_optical_frame").get_parameter_value().string_value self.camera_active = True @@ -110,6 +117,7 @@ def __init__(self): {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_ros_joint_states_event())}, {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_imu_event())}, {"frequency": 4, "handler": lambda: self.publish(lambda robot: robot.publish_true_pose_event())}, + {"frequency": 4, "handler": self.publish_true_ball_event}, {"frequency": 32, "handler": lambda: self.publish(lambda robot: robot.publish_camera_event())}, ] @@ -406,6 +414,16 @@ def publish_clock_event(self) -> None: clock_msg.clock = self.time_message self.clock_publisher.publish(clock_msg) + def publish_true_ball_event(self) -> None: + if self.ball_body_id is None: + return + pos = self.data.xpos[self.ball_body_id] + msg = PointStamped() + msg.header.stamp = self.time_message + msg.header.frame_id = "map" + msg.point.x, msg.point.y, msg.point.z = float(pos[0]), float(pos[1]), float(pos[2]) + self.true_ball_publisher.publish(msg) + def publish(self, executor: Callable[["RobotSimulation"], None]) -> None: for robot in self.robots: executor(robot) From 708fa5d0a09cad40b1d06817075ebda5800f133f Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Fri, 3 Jul 2026 21:39:12 +0900 Subject: [PATCH 3/5] Add DCD forwarding --- .../launch/mujoco_simulation.launch.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py b/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py index 39a73f5326..137b266f96 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py +++ b/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py @@ -81,6 +81,27 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path: "remap": f"{namespace}/joint_command", } + # Debug topics: robot domain -> main (reversed direction) + debug_topics = [ + ("/debug/dsd/body_behavior/dsd_current_action", "std_msgs/msg/String"), + ("/debug/dsd/body_behavior/dsd_stack", "std_msgs/msg/String"), + ("/debug/dsd/body_behavior/dsd_tree", "std_msgs/msg/String"), + ("/debug/dsd/hcm/dsd_current_action", "std_msgs/msg/String"), + ("/debug/dsd/hcm/dsd_stack", "std_msgs/msg/String"), + ("/debug/dsd/hcm/dsd_tree", "std_msgs/msg/String"), + ("/debug/dsd/localization/dsd_current_action", "std_msgs/msg/String"), + ("/debug/dsd/localization/dsd_stack", "std_msgs/msg/String"), + ("/debug/dsd/localization/dsd_tree", "std_msgs/msg/String"), + ] + for topic_suffix, msg_type in debug_topics: + src_topic = topic_suffix + config["topics"][src_topic] = { + "type": msg_type, + "remap": f"{namespace}{topic_suffix}", + "from_domain": robot_domain, + "to_domain": main_domain, + } + config_path = output_dir / f"robot{robot_domain}_bridge.yaml" with open(config_path, "w") as f: yaml.dump(config, f, default_flow_style=False, sort_keys=False) From e80ebff284bfb69f57a2edf7cce30987bf4680da Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Sat, 4 Jul 2026 12:42:36 +0900 Subject: [PATCH 4/5] Add node and topic monitoring --- scripts/robot_overview.py | 86 ++++++++++++++++++++++++++++++++++----- 1 file changed, 76 insertions(+), 10 deletions(-) diff --git a/scripts/robot_overview.py b/scripts/robot_overview.py index 4a0ed6e326..d748bf881c 100644 --- a/scripts/robot_overview.py +++ b/scripts/robot_overview.py @@ -105,6 +105,11 @@ class RobotState: ball_map: Stamped = field(default_factory=Stamped) # filtered ball, transformed to map by us +def _fq(namespace: str, name: str) -> str: + """Fully-qualified node name from a (namespace, name) pair, e.g. ('/', 'foo') -> '/foo'.""" + return f"{namespace.rstrip('/')}/{name}" + + class _MonitorBase: """A rclpy context + node + executor bound to a single ROS domain, spun in a thread.""" @@ -115,11 +120,39 @@ def __init__(self, domain: int, node_name: str): self.executor = SingleThreadedExecutor(context=self.context) self.executor.add_node(self.node) self._thread = threading.Thread(target=self._spin, daemon=True) + # Graph counts (nodes/topics in this domain, excluding ourselves), refreshed on a timer. + self.n_nodes: Optional[int] = None + self.n_topics: Optional[int] = None @staticmethod def _store(slot: Stamped): return lambda msg: slot.set(msg) + def refresh_counts(self) -> None: + """Recount the nodes and topics in this domain, excluding our own monitoring node. + + A topic is skipped only if the sole participant is us; a node/topic that anyone else + publishes, subscribes to, or hosts is counted. + """ + node = self.node + our = node.get_fully_qualified_name() + try: + self.n_nodes = sum( + 1 for name, ns in node.get_node_names_and_namespaces() if _fq(ns, name) != our + ) + topics = 0 + for topic, _types in node.get_topic_names_and_types(): + endpoints = { + _fq(info.node_namespace, info.node_name) + for info in node.get_publishers_info_by_topic(topic) + + node.get_subscriptions_info_by_topic(topic) + } + if endpoints - {our}: # someone other than us is on this topic + topics += 1 + self.n_topics = topics + except Exception: # noqa: BLE001 - graph queries can race with shutdown; keep last values + pass + def _spin(self): try: self.executor.spin() @@ -237,15 +270,21 @@ def _fmt_age(age: Optional[float]) -> str: return f"{age:4.1f}s" -def render(states: list[RobotState], true_ball: Optional[tuple[float, float]] = None, rotate_180: bool = True) -> str: +def render( + domain_monitors: list["DomainMonitor"], + true_ball: Optional[tuple[float, float]] = None, + rotate_180: bool = True, +) -> str: # true_ball is the sim ground-truth ball (map frame). ball_err is the distance between # the filtered ball (transformed to map) and the ground-truth ball (position only). + # nodes/topics count the other participants discovered in each robot's domain. header = ( f"{'robot':>6} | {'role':>8} | {'action':>15} | {'pose err (m,°)':^14} | " - f"{'ball err':>8} | {'ttb':>6} | {'game':>8} | {'pen':>3} | {'age':>5}" + f"{'ball err':>8} | {'ttb':>6} | {'game':>8} | {'pen':>3} | {'nds':>4} | {'tpc':>4} | {'age':>5}" ) lines = [header, "-" * len(header)] - for s in states: + for m in domain_monitors: + s = m.state strat: Optional[Strategy] = s.strategy.value gs: Optional[GameState] = s.gamestate.value ttb: Optional[Float32] = s.time_to_ball.value @@ -260,14 +299,15 @@ def render(states: list[RobotState], true_ball: Optional[tuple[float, float]] = ttb_str = f"{ttb.data:5.1f}s" if ttb else " - " game = GAME_STATE.get(gs.main_state, "?") if gs else "-" pen = ("Y" if gs.penalized else "n") if gs and hasattr(gs, "penalized") else "-" + nds = "-" if m.n_nodes is None else str(m.n_nodes) + tpc = "-" if m.n_topics is None else str(m.n_topics) # freshest signal we have from this robot -> its liveness ages = [a for a in (s.strategy.age(), s.pose.age(), s.gamestate.age()) if a is not None] age = _fmt_age(min(ages) if ages else None) lines.append( f"{s.domain:>6} | {role:>8} | {action:>15} | {_fmt_error(est, true):^14} | " - f"{ball_err:>8} | " - f"{ttb_str:>6} | {game:>8} | {pen:>3} | {age:>5}" + f"{ball_err:>8} | {ttb_str:>6} | {game:>8} | {pen:>3} | {nds:>4} | {tpc:>4} | {age:>5}" ) return "\n".join(lines) @@ -285,6 +325,12 @@ def parse_args(): help="Domain the sim runs in, where robotN/true_pose is published (default: $ROS_DOMAIN_ID or 0)", ) ap.add_argument("--once", action="store_true", help="Collect briefly, print one snapshot, then exit") + ap.add_argument( + "--count-interval", + type=float, + default=3.0, + help="How often (seconds) to recount nodes/topics per domain (default: 3)", + ) ap.add_argument( "--rotate-180", action=argparse.BooleanOptionalAction, @@ -306,9 +352,9 @@ def main(): domains = [11, 12, 13, 14] states = [RobotState(domain=d) for d in domains] + domain_monitors = [DomainMonitor(s) for s in states] truth = GroundTruthMonitor(states, args.main_domain) - monitors: list[_MonitorBase] = [DomainMonitor(s) for s in states] - monitors.append(truth) + monitors: list[_MonitorBase] = [*domain_monitors, truth] for m in monitors: m.start() @@ -318,22 +364,42 @@ def true_ball_xy() -> Optional[tuple[float, float]]: return xy return (-xy[0], -xy[1]) # same 180° realignment as the ground-truth pose + last_count = 0.0 + + def maybe_refresh_counts() -> None: + nonlocal last_count + now = time.monotonic() + if now - last_count >= args.count_interval: + for m in monitors: + m.refresh_counts() + last_count = now + + def global_line() -> str: + nds = "-" if truth.n_nodes is None else truth.n_nodes + tpc = "-" if truth.n_topics is None else truth.n_topics + return f"global namespace (domain {args.main_domain}): nodes={nds} topics={tpc}" + try: if args.once: time.sleep(1.0) # let subscriptions match and receive - print(render(states, true_ball_xy(), args.rotate_180)) + maybe_refresh_counts() + print(render(domain_monitors, true_ball_xy(), args.rotate_180)) + print("\n" + global_line()) else: period = 1.0 / args.rate while True: + maybe_refresh_counts() # clear screen + home cursor, then redraw the table in place print("\033[2J\033[H", end="") rot = " (truth rotated 180°)" if args.rotate_180 else "" print( f"Robot overview domains={domains} truth@{args.main_domain}{rot} " f"({time.strftime('%H:%M:%S')})\n" - "ball err = filtered ball (transformed odom->map) vs true ball\n" + "ball err = filtered ball (transformed odom->map) vs true ball | " + "nds/tpc = other nodes/topics in domain\n" ) - print(render(states, true_ball_xy(), args.rotate_180)) + print(render(domain_monitors, true_ball_xy(), args.rotate_180)) + print("\n" + global_line()) time.sleep(period) except KeyboardInterrupt: pass From 04c0dc91063e4c89bdcbf3ba08223cada4ecb13c Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Sat, 4 Jul 2026 12:53:57 +0900 Subject: [PATCH 5/5] Add simulation reset handling --- scripts/robot_overview.py | 43 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/scripts/robot_overview.py b/scripts/robot_overview.py index d748bf881c..b4ef9baf94 100644 --- a/scripts/robot_overview.py +++ b/scripts/robot_overview.py @@ -39,6 +39,7 @@ from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.time import Time +from rosgraph_msgs.msg import Clock from std_msgs.msg import Float32, Header from bitbots_msgs.msg import Strategy @@ -87,6 +88,10 @@ def set(self, value): self.value = value self.at = time.monotonic() + def clear(self): + self.value = None + self.at = 0.0 + def age(self) -> Optional[float]: return None if self.value is None else time.monotonic() - self.at @@ -123,11 +128,24 @@ def __init__(self, domain: int, node_name: str): # Graph counts (nodes/topics in this domain, excluding ourselves), refreshed on a timer. self.n_nodes: Optional[int] = None self.n_topics: Optional[int] = None + # Watch the clock so we can drop stale state/transforms when the sim restarts. + self._last_clock: Optional[float] = None + self.node.create_subscription(Clock, "/clock", self._clock_cb, 1) @staticmethod def _store(slot: Stamped): return lambda msg: slot.set(msg) + def _clock_cb(self, msg: Clock) -> None: + t = msg.clock.sec + msg.clock.nanosec * 1e-9 + # A large backward jump landing near zero means the simulation was restarted. + if self._last_clock is not None and t < self._last_clock - 1.0 and t < 5.0: + self._on_reset() + self._last_clock = t + + def _on_reset(self) -> None: + """Hook: drop cached data that is invalid after a sim restart. Overridden below.""" + def refresh_counts(self) -> None: """Recount the nodes and topics in this domain, excluding our own monitoring node. @@ -201,6 +219,25 @@ def _ball_filtered_cb(self, msg: PoseWithCovarianceStamped) -> None: except tf2_ros.TransformException: pass # TF not ready yet / frame missing — keep the previous value + def _on_reset(self) -> None: + # Clear the TF buffer so pre-restart transforms (stamped at the old, high sim time) + # cannot shadow the fresh ones, and drop the cached per-robot state. + try: + self.tf_buffer.clear() + except AttributeError: + # Bindings without BufferCore.clear(): rebuild buffer + listener instead. + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + for slot in ( + self.state.strategy, + self.state.pose, + self.state.gamestate, + self.state.time_to_ball, + self.state.cmd_vel, + self.state.ball_map, + ): + slot.clear() + class GroundTruthMonitor(_MonitorBase): """Reads the sim's ground truth from the main domain (never bridged): per-robot @@ -208,6 +245,7 @@ class GroundTruthMonitor(_MonitorBase): def __init__(self, states: list[RobotState], main_domain: int): super().__init__(main_domain, "robot_overview_truth") + self.states = states self.true_ball = Stamped() # single global ball, shared across robots self.node.create_subscription(PointStamped, "true_ball", self._store(self.true_ball), 1) for state in states: @@ -215,6 +253,11 @@ def __init__(self, states: list[RobotState], main_domain: int): PoseStamped, f"robot{state.domain}/true_pose", self._store(state.true_pose), 1 ) + def _on_reset(self) -> None: + self.true_ball.clear() + for state in self.states: + state.true_pose.clear() + # ---- Rendering --------------------------------------------------------------------