first_frame.py

#
# MIT License
#
# Copyright (c) 2025 Analog Devices, Inc.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#

import aditofpython as tof
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import sys
import os
import struct
import argparse

parser = argparse.ArgumentParser(
    description="ADCAM First Frame Example - Capture and save/display ToF frames",
    formatter_class=argparse.RawDescriptionHelpFormatter,
    epilog="""
Examples:
  # Target with mode 2, show visualization
  python %(prog)s -m 2 -o show

  # Network with mode 3, save snapshot
  python %(prog)s -m 3 -i 192.168.56.1 -o snap

  # Target with mode 5, dump binary files
  python %(prog)s -m 5 -o bin
"""
)
parser.add_argument('-m', '--mode', type=int, required=True,
                    help='Frame mode number (0-6)')
parser.add_argument('-i', '--ip', type=str, default='',
                    help='IP address for network connection (omit for target)')
parser.add_argument('-o', '--output', type=str, choices=['bin', 'snap', 'show'], default='show',
                    help='Output type: bin=binary dumps, snap=snapshot files, show=visualization (default: show)')

args = parser.parse_args()

mode = args.mode
ip = args.ip
output_type = args.output

system = tof.System()

print("SDK version: ", tof.getApiVersion(), " | branch: ", tof.getBranchVersion(), " | commit: ", tof.getCommitVersion())

cameras = []
if ip:
    print(f"Looking for camera on network @ {ip}.")
    ip = "ip:" + ip
    status = system.getCameraList(cameras, ip)
else:
    print("Looking for camera on Target.")
    status = system.getCameraList(cameras)
print("system.getCameraList()", status)

camera1 = cameras[0]

#create callback and register it to the interrupt routine
def callbackFunction(callbackStatus):
    print("Running the python callback for which the status of ADSD3500 has been forwarded. ADSD3500 status = ", callbackStatus)

sensor = camera1.getSensor()
status = sensor.adsd3500_register_interrupt_callback(callbackFunction)

status = camera1.initialize()
print("camera1.initialize()", status)

modes = []
status = camera1.getAvailableModes(modes)
print("camera1.getAvailableModes()", status)
print(modes)

if int(mode) not in modes:
    print(f"Error: Unknown mode - {mode}")
    exit(-3)

camDetails = tof.CameraDetails()
status = camera1.getDetails(camDetails)
print("camera1.getDetails()", status)
print("camera1 details:", "id:", camDetails.cameraId, "connection:", camDetails.connection)

status = camera1.setMode(int(mode))
print("camera1.setMode(",mode,")", status)

# Example of getting/modifying/setting the current ADSD3500 parameters
# status, currentFrameProcessParams = camera1.getFrameProcessParams()
# currentFrameProcessParams["ab_thresh_min"] = "4"
# camera1.setFrameProcessParams(currentFrameProcessParams)

# Example of configuring the Dynamic Mode Switching
# The expected sequence of frame is: mode2, mode2, mode2, mode3, mode2, ...
# camera1.adsd3500setEnableDynamicModeSwitching(True)
# camera1.adsds3500setDynamicModeSwitchingSequence([(2, 3), (3, 1)])

status = camera1.start()
print("camera1.start()", status)

frame = tof.Frame()

status = camera1.requestFrame(frame)
print("camera1.requestFrame()", status)

frameDataDetails = tof.FrameDataDetails()
status = frame.getDataDetails("depth", frameDataDetails)
print("frame.getDataDetails()", status)
print("depth frame details:", "width:", frameDataDetails.width, "height:", frameDataDetails.height, "type:", frameDataDetails.type)

status = camera1.stop()
print("camera1.stop()", status)

# Get frame details to check which data types are available
frameDetails = tof.FrameDetails()
status = frame.getDetails(frameDetails)

# Build a list of available frame types
available_types = [detail.type for detail in frameDetails.dataDetails]
print(f"Available frame types: {available_types}")

# Get frame data - only retrieve what's available
image_depth = None
image_ab = None
image_conf = None
image_xyz = None

# Get the depth frame
if "depth" in available_types:
    image_depth = np.asarray(frame.getData("depth"))
    print("Depth frame retrieved")
else:
    print("Warning: Depth frame not available")

# Get the AB frame
if "ab" in available_types:
    image_ab = np.asarray(frame.getData("ab"))
    print("AB frame retrieved")
else:
    print("Warning: AB frame not available (bitsInAB=0)")

# Get the confidence frame
if "conf" in available_types:
    image_conf = np.asarray(frame.getData("conf"))
    image_conf2 = image_conf.flatten()
    count = 0
    
    final_conf = np.zeros(frameDataDetails.width*frameDataDetails.height*4)
    for i in range(frameDataDetails.width*(frameDataDetails.height // 2)):
        packed_float = struct.pack('f', image_conf2[i])

    # Unpack the bytes into four uint8 values
        uint8_values = struct.unpack('2H', packed_float)
        array_data = np.array(uint8_values)
        for j in range(2):
            final_conf[count+j] = array_data[j]
            #print(j)
        count = count + 2
    image_conf = np.reshape(final_conf[frameDataDetails.width*frameDataDetails.height*0:frameDataDetails.width*frameDataDetails.height*1], \
                            [frameDataDetails.height,frameDataDetails.width])
    print("Confidence frame retrieved")
else:
    print("Warning: Confidence frame not available (bitsInConf=0)")

# Get the XYZ frame
if "xyz" in available_types:
    image_xyz = np.asarray(frame.getData("xyz"))
    print("XYZ frame retrieved")
else:
    print("Warning: XYZ frame not available (xyzEnable=0)")

metadata = tof.Metadata
status, metadata = frame.getMetadataStruct()

#Unregister callback
status = sensor.adsd3500_unregister_interrupt_callback(callbackFunction)

# Metadata values
sensor_temp = metadata.sensorTemperature
laser_temp = metadata.laserTemperature
frame_num = metadata.frameNumber
imager_mode = metadata.imagerMode

print("Sensor temperature from metadata: ", sensor_temp)
print("Laser temperature from metadata: ", laser_temp)
print("Frame number from metadata: ", frame_num)
print("Mode from metadata: ", imager_mode)

if output_type == 'show':
    # Count available frames and create dynamic layout
    subplot_idx = 1
    
    # Create a figure with dynamic subplots
    fig = plt.figure(figsize=(20, 8))
    
    # Plot the depth image if available
    if image_depth is not None:
        ax1 = fig.add_subplot(2, 3, subplot_idx)
        im1 = ax1.imshow(image_depth, cmap='jet')
        ax1.set_title("Depth Image")
        ax1.axis("off")
        fig.colorbar(im1, ax=ax1)
        subplot_idx += 1

    # Plot the AB image if available
    if image_ab is not None:
        ax2 = fig.add_subplot(2, 3, subplot_idx)
        im2 = ax2.imshow(image_ab, cmap='gray')
        ax2.set_title("AB Image")
        ax2.axis("off")
        fig.colorbar(im2, ax=ax2)
        subplot_idx += 1

    # Plot the Confidence image if available
    if image_conf is not None:
        ax3 = fig.add_subplot(2, 3, subplot_idx)
        im3 = ax3.imshow(image_conf, cmap='gray')
        ax3.set_title("Confidence Image")
        ax3.axis("off")
        fig.colorbar(im3, ax=ax3)
        subplot_idx += 1

    # Metadata as text
    ax4 = fig.add_subplot(2, 3, subplot_idx)
    ax4.axis("off")  # Hide axes
    metadata_text = (
        f"Sensor Temp: {sensor_temp}°C\n"
        f"Laser Temp: {laser_temp}°C\n"
        f"Frame #: {frame_num}\n"
        f"Mode: {imager_mode}\n"
    )
    ax4.text(0.1, 0.5, metadata_text, fontsize=12, verticalalignment='center')
    subplot_idx += 1

    plt.tight_layout()
    plt.show()
elif output_type == 'snap':
    handler = tof.FrameHandler()
    base_name = "snapshot_mode_" + str(mode)
    status = handler.SnapShotFrames(base_name, frame)
    print("FrameHandler.SnapShotFrames()", status)
elif output_type == 'bin':
    # dump the files - only save frames that are available
    if image_depth is not None:
        image_depth.tofile("depth_mode_" + str(mode) + ".bin")
        print(f"Saved: depth_mode_{mode}.bin")
    if image_ab is not None:
        image_ab.tofile("ab_mode_" + str(mode) + ".bin")
        print(f"Saved: ab_mode_{mode}.bin")
    if image_conf is not None:
        image_conf.tofile("conf_mode_" + str(mode) + ".bin")
        print(f"Saved: conf_mode_{mode}.bin")
    if image_xyz is not None:
        image_xyz.tofile("xyz_mode_" + str(mode) + ".bin")
        print(f"Saved: xyz_mode_{mode}.bin")