Clean Go2 Static Transform frames#2236
Conversation
|
|
||
|
|
||
| # base_link → camera_optical extrinsics (applied at render time for image observations) | ||
| _BASE_TO_OPTICAL = Transform( |
There was a problem hiding this comment.
NOTE: I'm not fixing this hardcoded problem, just trying to clean up the go2 side
|
|
||
| self._camera_info_thread = Thread( | ||
| target=self.publish_camera_info, | ||
| self._static_publish_thread = Thread( |
There was a problem hiding this comment.
upgrade from just camera to all static info
|
|
||
| # Static camera mount chain: base_link -> camera_link -> camera_optical. | ||
| # TODO we need a standardized way to specify this for all cameras in dimos | ||
| BASE_TO_OPTICAL: Transform = Transform( |
There was a problem hiding this comment.
these were moved to the urdf
Greptile SummaryThis PR centralises Go2 frame definitions into a new
Confidence Score: 5/5Safe to merge — the previously reported crashes and broken imports are all resolved, and the new URDF-driven transform logic is mathematically equivalent to the constants it replaces. The refactor cleanly separates dynamic (odom) and static (camera/lidar) TF publishing, the RPY→quaternion conversion in go2.urdf produces the same values as the removed hardcoded constants, and all previously broken import/classmethod signatures are fixed. Only minor annotation and header nits remain. dimos/robot/unitree/go2/config.py — misleading parameter name and duplicate license header; dimos/robot/unitree/dimsim_connection.py — passes PoseStamped to a function typed Odometry. Important Files Changed
Sequence DiagramsequenceDiagram
participant GO2Connection
participant _static_publish thread
participant TF
participant camera_info Out
GO2Connection->>_static_publish thread: start (daemon)
Note over _static_publish thread: build remap dict once<br/>(Go2Config.body_frame → self.frame_id,<br/>**frame_mapping)
Note over _static_publish thread: build stamped_statics list once<br/>(apply remap to frame IDs)
loop every 1/static_publish_rate seconds
_static_publish thread->>TF: publish(camera_link, camera_optical, lidar_link transforms with ts=now)
_static_publish thread->>camera_info Out: publish(camera_info_static)
end
GO2Connection->>GO2Connection: _publish_tf(PoseStamped)
GO2Connection->>TF: publish(world→base_link with ts from msg)
Reviews (9): Last reviewed commit: "-" | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
Codecov Report❌ Patch coverage is 📢 Thoughts on this report? Let us know! |
…removed GO2Connection._odom_to_tf PR #2236 removed the GO2Connection._odom_to_tf classmethod in favor of URDF-derived static transforms via Go2Config. Also removed the connection._camera_info_static() module-level helper in favor of camera_info_static() in dimos.robot.unitree.go2.config. The detection conftest still called both, so the get_moment fixture broke (locally, since test_detection2d carries @pytest.mark.skipif_in_ci). Build the transform list inline (world/base_link from the odom pose plus the static fixed-joint transforms from Go2Config.static_transforms with the odom timestamp stamped on) and switch to the new camera_info_static() location.
Records solutions for the four greptile-apps review comments on this PR: - P1 #3293654340 (dict merge order in _static_publish) — fixed - P2 #3293617061 (typo 'remape') — naturally resolved by Jeff's 'improve' commit which rewrote the surrounding code, comment line no longer exists - P2 #3293617047 (hardcoded key in rerun.py) — skipped, Jeff already acknowledged inline that this is an out-of-scope cleanup - P2 #3293617084 (path_to walks non-fixed joints) — skipped, only affects non-Go2 robots with floating-joint URDFs, belongs in a follow-up RobotConfig PR
Use Go2Config.static_transforms + local _odom_to_tf helper, and the public camera_info_static() (was the removed _camera_info_static).
| if img is not None: | ||
| # Apply base→optical extrinsics for camera frustum rendering | ||
| world_T_optical = Transform.from_pose("world", obs.pose_stamped) + _BASE_TO_OPTICAL | ||
| # TODO: fix there should be a standard way to set camera frames |
There was a problem hiding this comment.
(this is not a new TODO)
| from dimos.protocol.tf.tf import TF | ||
| from dimos.robot.unitree.go2 import connection | ||
| from dimos.robot.unitree.go2.config import camera_info_static | ||
| from dimos.robot.unitree.go2.connection import connection |
There was a problem hiding this comment.
ImportError at module load — connection is not a name in the module
from dimos.robot.unitree.go2.connection import connection looks for a symbol named connection inside the connection module. No such module-level name exists there (the connection local only appears inside deploy()). This will raise ImportError: cannot import name 'connection' from 'dimos.robot.unitree.go2.connection' before a single test runs. The original form from dimos.robot.unitree.go2 import connection (importing the module itself) is what the subsequent connection.GO2Connection._odom_to_tf call actually needs.
| @classmethod | ||
| def _odom_to_tf(cls, odom: PoseStamped) -> list[Transform]: | ||
| camera_link = Transform( | ||
| translation=Vector3(0.3, 0.0, 0.0), | ||
| rotation=Quaternion(0.0, 0.0, 0.0, 1.0), | ||
| frame_id="base_link", | ||
| child_frame_id="camera_link", | ||
| ts=odom.ts, | ||
| def _odom_to_tf(cls: Odometry) -> list[Transform]: | ||
| """[world→base_link, base_link→camera_link, camera_link→camera_optical] for tests.""" | ||
| base_link = Transform( | ||
| translation=cls.position, | ||
| rotation=cls.orientation, | ||
| frame_id=cls.frame_id or DEFAULT_WORLD_FRAME, | ||
| child_frame_id=DEFAULT_ROBOT_FRAME, | ||
| ts=cls.ts, | ||
| ) | ||
|
|
||
| camera_optical = Transform( | ||
| translation=Vector3(0.0, 0.0, 0.0), | ||
| rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), | ||
| frame_id="camera_link", | ||
| child_frame_id="camera_optical", | ||
| ts=odom.ts, | ||
| ) | ||
|
|
||
| return [ | ||
| Transform.from_pose("base_link", odom), | ||
| camera_link, | ||
| camera_optical, | ||
| statics = [ | ||
| Transform( | ||
| translation=t.translation, | ||
| rotation=t.rotation, | ||
| frame_id=t.frame_id, | ||
| child_frame_id=t.child_frame_id, | ||
| ts=cls.ts, | ||
| ) | ||
| for t in ( | ||
| Go2Config.static_transforms["camera_link"], | ||
| Go2Config.static_transforms["camera_optical"], | ||
| ) | ||
| ] | ||
| return [base_link, *statics] |
There was a problem hiding this comment.
@classmethod + cls: Odometry is a broken signature — TypeError at call site
@classmethod causes Python to automatically pass the class (GO2Connection) as the first argument. The method has only one declared parameter (cls), so any call like GO2Connection._odom_to_tf(odom) raises TypeError: _odom_to_tf() takes 1 positional argument but 2 were given. The type annotation cls: Odometry makes it look like a regular function but the decorator means cls is always the class. test_moment.py:55 still calls this method directly, so this failure is live. The decorator should be @staticmethod and the first parameter renamed to odom: Odometry.
| @classmethod | |
| def _odom_to_tf(cls, odom: PoseStamped) -> list[Transform]: | |
| camera_link = Transform( | |
| translation=Vector3(0.3, 0.0, 0.0), | |
| rotation=Quaternion(0.0, 0.0, 0.0, 1.0), | |
| frame_id="base_link", | |
| child_frame_id="camera_link", | |
| ts=odom.ts, | |
| def _odom_to_tf(cls: Odometry) -> list[Transform]: | |
| """[world→base_link, base_link→camera_link, camera_link→camera_optical] for tests.""" | |
| base_link = Transform( | |
| translation=cls.position, | |
| rotation=cls.orientation, | |
| frame_id=cls.frame_id or DEFAULT_WORLD_FRAME, | |
| child_frame_id=DEFAULT_ROBOT_FRAME, | |
| ts=cls.ts, | |
| ) | |
| camera_optical = Transform( | |
| translation=Vector3(0.0, 0.0, 0.0), | |
| rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), | |
| frame_id="camera_link", | |
| child_frame_id="camera_optical", | |
| ts=odom.ts, | |
| ) | |
| return [ | |
| Transform.from_pose("base_link", odom), | |
| camera_link, | |
| camera_optical, | |
| statics = [ | |
| Transform( | |
| translation=t.translation, | |
| rotation=t.rotation, | |
| frame_id=t.frame_id, | |
| child_frame_id=t.child_frame_id, | |
| ts=cls.ts, | |
| ) | |
| for t in ( | |
| Go2Config.static_transforms["camera_link"], | |
| Go2Config.static_transforms["camera_optical"], | |
| ) | |
| ] | |
| return [base_link, *statics] | |
| @staticmethod | |
| def _odom_to_tf(odom: Odometry) -> list[Transform]: | |
| """[world→base_link, base_link→camera_link, camera_link→camera_optical] for tests.""" | |
| base_link = Transform( | |
| translation=odom.position, | |
| rotation=odom.orientation, | |
| frame_id=odom.frame_id or DEFAULT_WORLD_FRAME, | |
| child_frame_id=DEFAULT_ROBOT_FRAME, | |
| ts=odom.ts, | |
| ) | |
| statics = [ | |
| Transform( | |
| translation=t.translation, | |
| rotation=t.rotation, | |
| frame_id=t.frame_id, | |
| child_frame_id=t.child_frame_id, | |
| ts=odom.ts, | |
| ) | |
| for t in ( | |
| Go2Config.static_transforms["camera_link"], | |
| Go2Config.static_transforms["camera_optical"], | |
| ) | |
| ] | |
| return [base_link, *statics] |
| from dimos.msgs.nav_msgs.Odometry import Odometry | ||
| from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo | ||
| from dimos.robot.config import RobotConfig | ||
| from dimos.robot.unitree.go2.config import Go2Config, camera_info_static |
There was a problem hiding this comment.
Line 39 imports Go2Config and camera_info_static from dimos.robot.unitree.go2.config — which is this very file. At the time Python executes this statement, neither name is defined yet (camera_info_static is at line 46, Go2Config at line 77). Python raises ImportError: cannot import name 'Go2Config' from partially initialized module 'dimos.robot.unitree.go2.config', which cascades to connection.py, conftest.py, rerun.py, and test_moment.py — every file that imports from this module. The line should simply be deleted; everything needed is already defined in the same file.
1 participant
self.frame_idNOTE: I don't want this to become a whole static-publishing system - just trying to make go2 a bit cleaner
How to Test
Contributor License Agreement