diff --git a/scripts/robot_overview.py b/scripts/robot_overview.py new file mode 100644 index 000000000..b4ef9baf9 --- /dev/null +++ b/scripts/robot_overview.py @@ -0,0 +1,455 @@ +#!/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 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 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 rclpy.time import Time +from rosgraph_msgs.msg import Clock +from std_msgs.msg import Float32, Header + +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 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 + + +@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) + 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.""" + + 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) + # 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. + + 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() + 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, 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 + + 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 + ``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.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: + self.node.create_subscription( + 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 -------------------------------------------------------------------- + + +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_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 _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 " - " + if age > 5.0: + return f"{age:4.0f}s" # clearly stale + return f"{age:4.1f}s" + + +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} | {'nds':>4} | {'tpc':>4} | {'age':>5}" + ) + lines = [header, "-" * len(header)] + 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 + 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 "-" + 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 "-" + 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} | {ttb_str:>6} | {game:>8} | {pen:>3} | {nds:>4} | {tpc:>4} | {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( + "--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, + 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] + domain_monitors = [DomainMonitor(s) for s in states] + truth = GroundTruthMonitor(states, args.main_domain) + monitors: list[_MonitorBase] = [*domain_monitors, 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 + + 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 + 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 | " + "nds/tpc = other nodes/topics in domain\n" + ) + print(render(domain_monitors, true_ball_xy(), args.rotate_180)) + print("\n" + global_line()) + time.sleep(period) + except KeyboardInterrupt: + pass + finally: + for m in monitors: + m.shutdown() + + +if __name__ == "__main__": + main() 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 39a73f532..137b266f9 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) 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 437d64089..386803571 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 7116cb860..d4889513b 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 476d9e148..c58d1bc78 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 PointStamped, PoseStamped from mujoco import viewer from rclpy.node import Node from rclpy.time import Time @@ -110,6 +111,13 @@ def __init__(self): self.create_service(Empty, "/reset_ball", self.reset_ball_callback) self.create_service(MoveBall, "/move_ball", self.move_ball_callback) + # 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 @@ -118,6 +126,8 @@ 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": 4, "handler": self.publish_true_ball_event}, {"frequency": 32, "handler": lambda: self.publish(lambda robot: robot.publish_camera_event())}, ] @@ -455,6 +465,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) @@ -477,6 +497,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) @@ -533,6 +557,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