Skip to content

feat(nav): add clicked_point input to replanning planner#1394

Open
spomichter wants to merge 17 commits intodevfrom
feat/click-to-nav
Open

feat(nav): add clicked_point input to replanning planner#1394
spomichter wants to merge 17 commits intodevfrom
feat/click-to-nav

Conversation

@spomichter
Copy link
Contributor

adds clicked_point: In[PointStamped] to ReplanningAStarPlanner. subscribes and converts to PoseStamped via .to_pose_stamped(), feeds to existing goal handler.

what

3 lines of code in the planner module — no new modules, no new blueprints, no custom transports.

why

the rerun viewer fork (dimensionalOS/rerun#1) publishes click coordinates as PointStamped to /clicked_point via LCM. this lets the planner receive those clicks as navigation goals.

usage

any blueprint using the planner can wire it:

blueprint.transports({
    ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped),
})

depends on

DIM-643

spomichter and others added 7 commits March 1, 2026 16:05
adds geometry_msgs.PointStamped following the PoseStamped pattern:
- inherits from dimos_lcm PointStamped + Timestamped mixin
- lcm_encode/lcm_decode for binary roundtrip
- to_rerun() → rr.Points3D
- to_pose_stamped() → PoseStamped with identity orientation
- plum.dispatch constructors (x/y/z, Vector3, list, kwargs)
- conforms to DimosMsg protocol
- 30 unit tests

enables the rerun viewer click-to-navigate pipeline: viewer publishes
PointStamped to /clicked_point via LCM, any module subscribes via
LCMTransport and converts to PoseStamped for navigation.

DIM-643
- replace multi-dispatch constructors with single __init__(x, y, z, ts, frame_id)
- inherit from Point(LCMPoint) instead of Vector3(LCMVector3)
- add Point wrapper class for geometry_msgs.Point
- simplify tests to focus on LCM roundtrip per review feedback

addresses review comments from leshy on PR #1388
3 focused tests: lcm roundtrip, Point inheritance, PoseStamped conversion
Point is a separate LCM message type, should have its own file
like Vector3, Quaternion, Pose, etc.
leshy
leshy previously approved these changes Mar 1, 2026
@greptile-apps
Copy link
Contributor

greptile-apps bot commented Mar 1, 2026

Greptile Summary

This PR adds PointStamped message type support and wires it into ReplanningAStarPlanner to enable click-to-navigate functionality from the Rerun viewer.

Key changes:

  • Introduces Point and PointStamped message types following the existing pattern of TwistStamped and PoseStamped (inheriting from base type + Timestamped mixin)
  • PointStamped.to_pose_stamped() converts clicked points to navigation goals with identity orientation
  • Planner module adds clicked_point: In[PointStamped] input and subscribes to convert incoming clicks to PoseStamped goals
  • Comprehensive test coverage for LCM encoding/decoding and conversion logic
  • Uses ros_timestamp() from Timestamped mixin, consistent with modern codebase patterns

Implementation quality:

  • Clean implementation following established patterns
  • Proper disposal management for subscriptions
  • Well-tested with roundtrip encoding tests
  • No breaking changes to existing functionality

Confidence Score: 5/5

  • This PR is safe to merge with no issues found
  • Clean implementation following existing patterns (PoseStamped, TwistStamped), proper test coverage, no breaking changes, correct disposal management, and straightforward integration with existing goal handler
  • No files require special attention

Important Files Changed

Filename Overview
dimos/msgs/geometry_msgs/PointStamped.py implements PointStamped message type with LCM encoding/decoding and to_pose_stamped conversion
dimos/msgs/geometry_msgs/test_PointStamped.py comprehensive tests for PointStamped LCM roundtrip and PoseStamped conversion
dimos/navigation/replanning_a_star/module.py adds clicked_point input stream and subscribes to convert clicks to navigation goals

Sequence Diagram

sequenceDiagram
    participant Rerun as Rerun Viewer
    participant LCM as LCM Transport
    participant Planner as ReplanningAStarPlanner
    participant Handler as GlobalPlanner.handle_goal_request

    Rerun->>LCM: publish PointStamped to /clicked_point
    LCM->>Planner: clicked_point.subscribe()
    Planner->>Planner: pt.to_pose_stamped()
    Note over Planner: converts PointStamped to PoseStamped<br/>with identity quaternion [0,0,0,1]
    Planner->>Handler: handle_goal_request(pose_stamped)
    Note over Handler: processes navigation goal
Loading

Last reviewed commit: 5f9845e

Copy link
Contributor

@greptile-apps greptile-apps bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

5 files reviewed, no comments

Edit Code Review Agent Settings | Greptile

- add type: ignore[misc] for LCMPoint subclass (generated code has Any type)
- add PoseStamped to TYPE_CHECKING import block
- fix test import: Point from Point module, not PointStamped module
subscribe to PointStamped clicks from rerun viewer, convert to
PoseStamped via .to_pose_stamped() and feed to goal handler.

wire with: LCMTransport("/clicked_point", PointStamped)

DIM-643
runs Go2 smart blueprint with viewer_mode=none, connects to custom
rerun viewer on gRPC port 9877, wires /clicked_point LCM transport
to planner's clicked_point input.

DIM-643
supports --simulation (mujoco), --robot-ip, --viewer-url args
rr.init + rr.connect_grpc must happen before the bridge starts,
otherwise rerun disables itself with no active sink
the bridge calls rr.init('dimos') in start(). calling rr.init before
that resets the recording stream. now we connect_grpc after start()
so the bridge's rr.init creates the stream, then we add our gRPC sink.
the bridge runs in a worker subprocess (multiprocessing), so its
rr.init() only affects that subprocess. the main process needs its
own rr.init() + connect_grpc() for data to reach the viewer.

also adds --replay flag for headless testing.
adds connect mode to rerun bridge — connects to an external viewer
via grpc instead of spawning a new one. needed for custom viewers
like the click-to-navigate fork.

example script now works like a normal blueprint.

DIM-643
proper blueprint registered in all_blueprints. run with:
  dimos run unitree-go2-click-nav --simulation --viewer-backend rerun-connect

adds rerun-connect viewer backend that connects to an external
rerun viewer (custom fork) instead of spawning a new one.

DIM-643
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants