Skip to content

Fix joint state REST parsing and add UGV startup example#18

Merged
cyberdepa merged 2 commits into
cyberwave-os:mainfrom
EdyVision:feature/examples-ugv-startup-calibration-pose
May 22, 2026
Merged

Fix joint state REST parsing and add UGV startup example#18
cyberdepa merged 2 commits into
cyberwave-os:mainfrom
EdyVision:feature/examples-ugv-startup-calibration-pose

Conversation

@EdyVision
Copy link
Copy Markdown
Contributor

Summary

Fixes parsing of twin GET joint-states so JointController.refresh() matches the OpenAPI JointStatesSchema (name / position lists). Adds a UGV bring-up example that exercises affect mode, optional MQTT motion for the pan-tilt head, readiness checks, and camera frame sampling.

Motivation

Joint state reads could stay empty or stuck at zero even when the twin exposed joints elsewhere, because refresh() only handled a legacy joint_states shape. The new example gives a single, copy-paste path for real-world vs simulation, MQTT update_joints_state (aggregated payload with timestamp), and env-driven head joint names and motion envelope.

Changes

  • cyberwave/twin.py: JointController.refresh() merges name/position when joint_states is not present.
  • examples/robot_startup_and_readiness.py: UGV startup flow (optional telemetry wake, joint summary with REST polling, optional asset pose, camera frames, optional head sequence); configurable via CYBERWAVE_* / UGV_* env.
  • tests/test_joint_controller_refresh.py: Covers legacy and list-shaped joint state responses.

Example Added: examples/robot_startup_and_readiness.py

Example Output:

python3.12 examples/robot_startup_and_readiness.py
twin: 1c04c936-aaa6-4a41-bfc3-47e9b5f3cf51 (existing twin_id)
telemetry wake: MQTT pan ±4.0° around neutral (hold 0.35s per step), then joint summary
  mqtt joints: pan=4.0 deg (0.0698 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
WARNING cyberwave.mqtt: MQTT connection settings: tls=True, broker=mqtt.cyberwave.com, port=8883, custom_ca=False
INFO cyberwave.mqtt: Connected to MQTT broker at mqtt.cyberwave.com:8883
INFO cyberwave.mqtt: Publishing telemetry start message for twin 1c04c936-aaa6-4a41-bfc3-47e9b5f3cf51: {'type': 'telemetry_start', 'timestamp': 1776785583.628138}
  mqtt joints: pan=-4.0 deg (-0.0698 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
joint check: 6 joint(s) online
joint check: note — all positions are still ~0 rad after MQTT wake and 5 REST poll(s). MQTT commands can still move the robot; this GET endpoint may not mirror tele/op state for this twin.
  left_down_wheel_link_joint: 0.0000 rad
  left_up_wheel_link_joint: 0.0000 rad
  pt_base_link_to_pt_link1: 0.0000 rad
  pt_link1_to_pt_link2: 0.0000 rad
  right_down_wheel_link_joint: 0.0000 rad
  right_up_wheel_link_joint: 0.0000 rad
startup pose: skipped (STARTUP_POSE_NAME is None)
camera readiness: capturing 3 frame(s) every 400 ms
camera readiness: warning — JPEG payloads are very small (min=78 bytes, median=78 bytes). A live stream can still be active elsewhere while /latest-frame returns a placeholder; try increasing MIN_JPEG_BYTES (now 2000) or verify media pipeline / sensor_id / cw.affect(...) mode.
candidate joints: left_up_wheel_link_joint, left_down_wheel_link_joint, right_up_wheel_link_joint, right_down_wheel_link_joint, pt_base_link_to_pt_link1, pt_link1_to_pt_link2
resolved pan joint: 'pt_base_link_to_pt_link1'
resolved tilt joint: 'pt_link1_to_pt_link2'
head startup: neutral pan=0.0 deg tilt=0.0 deg (pause 0.45s)
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
head startup: session cal pan left (-22.0 deg) …
  mqtt joints: pan=-22.0 deg (-0.3840 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
head startup: session cal pan right (22.0 deg) …
  mqtt joints: pan=22.0 deg (0.3840 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
head startup: session cal pan center (dead center) …
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
head startup: acknowledge nod ±5.0 deg at neutral tilt …
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=5.0 deg (command tilt=5.0 deg -> 0.0873 rad, invert=False)
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=-5.0 deg (command tilt=-5.0 deg -> -0.0873 rad, invert=False)
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=0.0 deg (command tilt=0.0 deg -> 0.0000 rad, invert=False)
head startup: final look-up tilt=20.0 deg (neutral + 20.0 deg)
  mqtt joints: pan=0.0 deg (0.0000 rad), tilt=20.0 deg (command tilt=20.0 deg -> 0.3491 rad, invert=False)
head startup: completed
startup sequence: done
INFO cyberwave.mqtt: publish_telemetry_end: twin 1c04c936-aaa6-4a41-bfc3-47e9b5f3cf51 was_in_tracking_list=True, remaining_tracked_twins=[]
INFO cyberwave.mqtt: Disconnecting from MQTT broker

@KhushiSharma22
Copy link
Copy Markdown
Collaborator

Hi @EdyVision
Thank you for your contribution! We'll review the same and will let you know of the updates!

Tagging @erlapso for a quick review on this one!

@cyberdepa
Copy link
Copy Markdown
Contributor

Hi @EdyVision , thanks for the great PR! 😄
Your contribution actually helped us spot a deeper issue with a legacy API. We've just gone ahead and replaced that method with a no-op and cleaned up the surrounding file, which has unfortunately triggered some merge conflicts here.
Since we definitely want to keep your excellent tests and examples, we are going to resolve the conflicts from our side—discarding the core code change in favor of the no-op cleanup, but keeping all of your test coverage.
Appreciate you bringing this to light and helping us make the codebase stronger!

@cyberdepa cyberdepa merged commit 1c6df4c into cyberwave-os:main May 22, 2026
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.

3 participants