astrateleop - dingdongdengdong/astra_ws GitHub Wiki

Okay, here are the simplified Mermaid diagrams and explanations for the astra_teleop files, focusing on the core functionality of each script.

Simplified Code Diagrams and Explanations for astra_teleop

This document provides simplified Mermaid diagrams and explanations for the Python files in the astra_teleop package, highlighting the main code flow.

astra_teleop/src/astra_teleop/cam.py

Opens and configures a camera.

graph TD
    A[Start] --> B[Open Camera];
    B --> C[Set Properties];
    C --> D[Return Camera Object];
    D --> E{If Main};
    E -- Yes --> F[Loop: Read, Display, Check Key];
    F --> G[Stop];
    E -- No --> G;

Explanation:

The cam.py script's main function is open_cam, which initializes a camera with specified settings like resolution and frame rate using OpenCV. When run directly, it continuously reads frames from the camera, displays them in a window, and stops if the 'Esc' key is pressed.

astra_teleop/src/astra_teleop/generate_aruco.py

Generates printable sheets of ArUco markers.

graph TD
    A[Start] --> B[Define Settings and Functions];
    B --> C[Generate Front Markers];
    C --> D[Save Front PDF];
    D --> E[Generate Back Markers];
    E --> F[Save Back PDF];
    F --> G[End];

Explanation:

This script creates PDF files containing ArUco markers designed for a target object. It defines the marker properties and layout for both the front and back sides. Functions get_marker_image and get_marker_image_back create individual marker images, which are then arranged into grids, formatted for A4 size, and saved as "marker_array_front.pdf" and "marker_array_back.pdf".

astra_teleop/src/astra_teleop/calibration_collect.py

Collects images from a camera for calibration.

graph TD
    A[Start] --> B[Open Camera];
    B --> C[Create Directory];
    C --> D{Loop: Collect Images};
    D --> E[Read Frame];
    E --> F[Show Frame];
    F --> G{Time to Save?};
    G -- Yes --> H[Save Image];
    H --> D;
    G -- No --> D;
    D -- Done --> I[End];

Explanation:

The calibration_collect.py script captures a series of images from a specified camera to be used for camera calibration. It opens the camera, ensures the output directory exists, and then enters a loop. In each iteration, it reads and displays a camera frame. If a set time interval has passed since the last saved image, it saves the current frame to the calibration directory and updates the timer. This process continues until a specified number of images are collected.

astra_teleop/src/astra_teleop/calibration_process.py

Processes calibration images to calculate camera parameters.

graph TD
    A[Start] --> B[Define Charuco Board and Detector];
    B --> C[Load Calibration Images];
    C --> D{Iterate Images};
    D --> E[Detect Charuco Corners/IDs];
    E --> F{Enough Detections?};
    F -- Yes --> G[Match Points];
    G --> H{Enough Matched Points?};
    H -- Yes --> I[Store Points];
    I --> J[Draw and Show Detections];
    J --> D;
    H -- No --> D;
    F -- No --> D;
    D -- All Images --> K[Perform Camera Calibration];
    K --> L[Save Results to YAML];
    L --> M[End];

Explanation:

This script performs the camera calibration by analyzing the images collected by calibration_collect.py. It defines a Charuco board and uses a detector to find the board corners and IDs in each image. For images with sufficient valid detections, it establishes correspondences between 3D points on the board and their 2D projections in the image. These points are used by OpenCV's calibration function to compute the camera's intrinsic matrix and distortion coefficients, which are then saved to a YAML file.

astra_teleop/src/astra_teleop/calibration_calculator.py

Loads calibration data and calculates camera FOV.

graph TD
    A[Start] --> B[Load Calibration Data];
    B --> C[Define FOV Calculation Function];
    C --> D[Calculate FOV];
    D --> E[Print FOV and Dimensions];
    E --> F[End];

Explanation:

The calibration_calculator.py script is a utility to inspect camera calibration results. It loads the camera matrix from a previously saved calibration file. It then uses this matrix to calculate the camera's horizontal, vertical, and diagonal fields of view (FOV) and image dimensions. These calculated values are printed to the console.

astra_teleop/src/astra_teleop/process.py

Core pipeline: camera capture, ArUco detection, and pose estimation.

graph TD
    A[Start] --> B{Call get_process};
    B --> C[Open Camera];
    C --> D[Load Calibration];
    D --> E[Setup Detector and Solver];
    E --> F[Return Process Function];
    F --> G{If Main};
    G -- Yes --> H{Loop_ Process Frames};
    H --> I[Read Frame];
    I --> J[Detect Markers];
    J --> K[Estimate Pose];
    K --> L[Return Poses];
    L --> M[Show Debug Info];
    M --> H;
    H -- Stop --> N[End];
    G -- No --> N;

Explanation:

The process.py script contains the main logic for real-time ArUco marker tracking. The get_process function sets up the camera, loads the calibration data, and initializes the marker detection and pose estimation components. It returns a process function which, when called, captures a frame, detects ArUco markers (specifically looking for designated left and right hand markers), and uses the camera calibration and predefined marker geometry to estimate the 3D pose of these markers relative to the camera. This process runs continuously when the script is executed directly, displaying the results in a debug window.

astra_teleop/src/astra_teleop/experiment/collect.py

Collects pose data over time during an experiment.

graph TD
    A[Start] --> B[Create Experiment Directory];
    B --> C[Open Data File];
    C --> D{Get Process Function};
    D --> E{Loop: Collect Data};
    E --> F[Get Poses from Process];
    F --> G[Get Timestamp];
    G --> H[Save Timestamp and Poses to File];
    H --> E;
    E -- Interrupt --> I[Close File];
    I --> J[End];

Explanation:

The experiment/collect.py script is used to gather data for experiments by recording the pose of the tracked ArUco markers over time. It takes arguments for the camera, calibration, and experiment directories. It creates the experiment directory and opens a JSONL file for writing. It obtains the processing function from astra_teleop.process and enters a loop where it continuously gets the current marker poses, records the timestamp, and writes this data as a JSON line to the file. The data collection continues until interrupted by the user.

astra_teleop/src/astra_teleop/experiment/optimize.py

Optimizes parameters to model recorded pose data.

graph TD
    A[Start] --> B[Load Data from data.jsonl];
    B --> C[Initialize Trainable Parameters];
    C --> D[Setup Optimizer];
    D --> E{Optimization Loop};
    E --> F[Compute Transformation Model];
    F --> G[Calculate Pose Loss];
    G --> H[Compute Total Loss];
    H --> I[Backward Pass];
    I --> J[Optimizer Step];
    J --> E;
    E -- Done --> K[Save Optimized Results];
    K --> L[Plot Trajectories];
    L --> M[End];

Explanation:

This script takes the pose data collected by experiment/collect.py and optimizes a set of parameters using PyTorch to find a mathematical model that best describes the observed motion. It loads the timestamps and poses, initializes parameters representing different transformation components, and sets up an optimizer. The optimization loop iteratively computes a transformation based on the current parameters and time, calculates the error between this model and the recorded poses, and updates the parameters to reduce the error. The optimized parameters and the resulting model's trajectory are saved, and a plot comparing the recorded and optimized trajectories is generated.

astra_teleop/src/astra_teleop/experiment/plot.py

Visualizes experiment results: errors and trajectories.

graph TD
    A[Start] --> B[Load Results from result_pt];
    B --> C[Plot Rotation Error];
    C --> D[Save Rotation Plot];
    D --> E[Plot Translation Error];
    E --> F[Save Translation Plot];
    F --> G[Plot 3D Trajectories];
    G --> H[Show Plots];
    H --> I[End];

Explanation:

The experiment/plot.py script visualizes the output of the optimization process from experiment/optimize.py. It loads the experiment results from result.pt, which include the recorded and optimized poses and the calculated errors. It generates two 2D plots showing the rotation and translation errors over time and saves them as PDF files. It also creates a 3D plot to visualize the recorded and optimized trajectories side-by-side, including a camera frame for reference, and displays this plot.

astra_teleop/src/astra_teleop/experiment/plot_final.py

Compares and plots errors from two experiment runs.

graph TD
    A[Start] --> B[Load RoboCu Results];
    B --> C[Load Vanilla Results];
    C --> D[Plot Comparative Rotation Error];
    D --> E[Save Rotation Plot];
    E --> F[Plot Comparative Translation Error];
    F --> G[Save Translation Plot];
    G --> H[Print Mean Errors];
    H --> I[End];

Explanation:

The experiment/plot_final.py script is specifically designed to compare the performance metrics (rotation and translation errors) from two distinct experiment runs, labeled "RoboCu" and "Vanilla". It loads the results from two predefined file paths. It then generates two plots, each showing the error over time, with the data from both experiment runs plotted on the same graph for direct comparison. The plots are saved as PDF files. Finally, it prints the mean errors for both datasets to provide a numerical summary of the comparison.