Work in progress: this documentation is still being written and reviewed. Check details against the source code.

Survey viewpoint teaching

Record operator-selected survey viewpoints from /joint_states and save them as a YAML pose registry.

scripts.survey_mapping.teach_survey_poses · Source

Perception and mapping · Guide · Symbol index

How it is used

Use the interactive main() entry point to save viewpoints while an operator positions the arm through the established robot controls. The teacher only observes /joint_states; it neither enables freedrive nor commands movement.

Enter saves the current six arm joints in degrees, u removes the latest saved entry, and q writes the YAML file. Choose an explicit output path to avoid replacing the bundled survey poses; --append loads existing entries first.

Start here

Callable

Purpose

main

Record viewpoints through an Enter/save, u/undo, q/finish loop, then write the remaining poses to YAML. Wait up to ten seconds for a complete arm state before starting; --append loads existing entries first.

PoseTeacher.snapshot_degrees

Read the last joint mapping and extract shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2 and wrist_3 in that order, converting radians to degrees rounded to two decimals.

format_poses

Format poses as a two-comment YAML document with each six-joint list on one line and two decimal places.

Functions

main()

Record viewpoints through an Enter/save, u/undo, q/finish loop, then write the remaining poses to YAML. Wait up to ten seconds for a complete arm state before starting; --append loads existing entries first.

Entry point · Source

main()

Returns: 0 after writing or when no poses remain; 1 for invalid existing data, state timeout or output error.

Errors: Catches load/write errors and KeyboardInterrupt/EOFError; interrupt/EOF still proceeds to save collected poses. Initial state timeout returns before file writing.

Side effects: Starts a daemon ROS spin thread, reads terminal input and /joint_states, and may overwrite –output. Existing output is not read without –append; cleanup destroys node and shuts down ROS.

format_poses()

Format poses as a two-comment YAML document with each six-joint list on one line and two decimal places.

Internal helper · Source

format_poses(poses)

Parameter

Meaning

poses

Ordered mapping of names to numeric joint sequences in degrees; names are emitted unquoted.

Returns: String ending in a newline; no sequence-length or YAML-key escaping validation.

Errors: Nonnumeric values can raise formatting errors.

Side effects: Creates text only.

Example

text = format_poses({"survey_1": [0, -90, 90, 0, 0, 0]})
assert "survey_1: [0.00, -90.00, 90.00, 0.00, 0.00, 0.00]" in text
Referenced in source (1)
  • main — scripts.survey_mapping.teach_survey_poses

Internal callbacks and helpers

load_existing()

Read a prior YAML pose mapping for append mode, requiring every entry to have a string name and exactly six float-convertible values.

Internal helper · Source

load_existing(path)

Parameter

Meaning

path

Path object for the YAML file; missing/nonfile paths yield an empty mapping.

Returns: Dictionary of names to six float degree values; empty/null YAML yields {}.

Errors: ValueError for any malformed document/entry; read/YAML errors propagate. Unlike run_survey.load_poses, nonfinite floats are not rejected.

Side effects: Reads the existing file without changing it.

Referenced in source (1)
  • main — scripts.survey_mapping.teach_survey_poses

next_index()

Find the lowest positive integer whose prefix_integer key is absent from poses, filling numbering gaps.

Internal helper · Source

next_index(poses, prefix)

Parameter

Meaning

poses

Mapping or container tested for existing generated keys.

prefix

Text used before the underscore in generated pose names.

Returns: First unused integer starting at 1.

Errors: Container membership errors propagate.

Example

assert next_index({"survey_1": [], "survey_3": []}, "survey") == 2
Referenced in source (1)
  • main — scripts.survey_mapping.teach_survey_poses

PoseTeacher

Cache joint-state messages and return the six arm joints in a fixed order, converted from radians to degrees and rounded to two decimal places.

Inherits from: Node.

PoseTeacher.snapshot_degrees()

Read the last joint mapping and extract shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2 and wrist_3 in that order, converting radians to degrees rounded to two decimals.

Method · Source

snapshot_degrees(self)

Returns: Six-element degree list, or None before a message or when any required joint is missing.

Errors: Missing joint logs an error and returns None; other conversion errors propagate. No age or finite-value validation is performed.

Side effects: Reads the cached mapping and may log an error; no motion.

PoseTeacher.__init__()

Create survey_pose_teacher with a depth-ten /joint_states subscription and a latest-joint cache.

Lifecycle · Source

__init__(self)

Errors: ROS setup errors propagate.

Side effects: Registers a ROS node/subscription and initializes the empty joint cache.

Referenced in source (1)
  • main — scripts.survey_mapping.teach_survey_poses

Internal callbacks and helpers

PoseTeacher._on_joints()

Replace the cached mapping with the latest paired joint names and positions.

Callback · Source

_on_joints(self, msg)

Parameter

Meaning

msg

JointState whose position values are radians; zip truncates mismatched name/position lengths.

Returns: None.

Errors: Malformed message access errors propagate.

Side effects: Updates the latest-joint cache; no motion.

Referenced in source (1)