# V15A Sigmoid function for distance activation 01-08-24
# V16A Calibration dial
# V16B USB Serial input
# V17A Calibartion file stor in state [2]Calibration
# V18A OPen Serial with exeption to start a serail thread or open a file
# V18D Included Nav Activation external Dial dials[2]/10
# V19A Sigmoid function with slope and mode control+quantized function
import serial
import socket
import threading
import time
from statemachine import StateMachine, State
import i2cdev
import numpy as np
import sys
import argparse
import jetson.utils
from jetson_inference import detectNet
from jetson_inference import depthNet
# from jetson_utils import videoSource, videoOutput, logUsage, cudaOverlay, cudaDeviceSynchronize
from depthnet_utils import depthBuffers
from jetson_utils import videoSource, videoOutput, logUsage, cudaOverlay, cudaImage, cudaConvertColor, cudaAllocMapped, cudaFromNumpy, cudaImage, cudaToNumpy, cudaFont
# Open Serial POrt for Calibration dials
# M8p = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
# Controller shared variables and Thread Lock
shared_variable = 0
variable_lock = threading.Lock()
# Init Bisplay (I2C)
pwm=i2cdev.I2C(0x40,1) # (PCA9685 address,I2C bus)
# Reset PWM
pwm.write(bytes([0xFA, 0])) # zero all pin
pwm.write(bytes([0xFB, 0])) # zero all pin
pwm.write(bytes([0xFC, 0])) # zero all pin
pwm.write(bytes([0xFD, 0])) # zero all pin
pwm.write(bytes([0x01, 0x04])) # The 16 LEDn outputs are configured with a totem pole structure.
pwm.write(bytes([0x00, 0x01])) #PCA9685 responds to LED All Call I2C-bus address
time.sleep(0.01) # wait for oscillator
'''
Set the PWM frequency to the provided value in hertz.
The maximum PWM frequency is 1526 Hz if the PRE_SCALE register is set "0x03h".
The minimum PWM frequency is 24 Hz if the PRE_SCALE register is set "0xFFh".
he PRE_SCALE register can only be set when the SLEEP bit of MODE1 register is set to logic 1.
'''
# Program to set the Duty cycle and frequency of PWM.
freq_hz=1000 # Frequency of PWM
freq_hz=freq_hz*0.9 #correction
prescale = int(np.floor(25000000.0/(4096.0*float(freq_hz))-1)) # datasheet equation
print("prescale = " , prescale )
pwm.write(bytes([0x00, 0x10]))
time.sleep(0.01)
pwm.write(bytes([0xFE, prescale]))
pwm.write(bytes([0x00, 0x80]))
time.sleep(0.01)
pwm.write(bytes([0x00, 0x00]))
time.sleep(0.01)
pwm.write(bytes([0x01, 0x04]))
time.sleep(0.01)
# Program to set the PWM for the channel.
def set_pwm(channel, value):
x=min(4095,value)
x=max(0,x)
"""Sets a single PWM channel."""
LED0_ON_L = 0x06
LED0_ON_H = 0x07
LED0_OFF_L = 0x08
LED0_OFF_H = 0x09
pwm.write(bytes([(LED0_ON_L+4*channel), 0]))
pwm.write(bytes([(LED0_ON_H+4*channel), 0]))
pwm.write(bytes([(LED0_OFF_L+4*channel), (x & 0xFF)]))
pwm.write(bytes([(LED0_OFF_H+4*channel), (x >> 8)]))
time.sleep(0.1)
# parse the command line
parser = argparse.ArgumentParser(description="Locate objects in a live camera stream using an object detection DNN.",
formatter_class=argparse.RawTextHelpFormatter,
epilog=detectNet.Usage() + videoSource.Usage() + videoOutput.Usage() + logUsage())
parser.add_argument("input_URI", type=str, default="", nargs='?', help="URI of the input stream")
parser.add_argument("output_URI", type=str, default="", nargs='?', help="URI of the output stream")
parser.add_argument("--network", type=str, default="ssd-mobilenet-v2", help="pre-trained model to load (see below for options)")
parser.add_argument("--overlay", type=str, default="box,labels,conf", help="detection overlay flags (e.g. --overlay=box,labels,conf)\nvalid combinations are: 'box', 'labels', 'conf', 'none'")
parser.add_argument("--threshold", type=float, default=0.5, help="minimum detection threshold to use")
parserd = argparse.ArgumentParser(description="Mono depth estimation on a video/image stream using depthNet DNN.",
formatter_class=argparse.RawTextHelpFormatter,
epilog=depthNet.Usage() + videoSource.Usage() + videoOutput.Usage() + logUsage())
parserd.add_argument("input_URI", type=str, default="", nargs='?', help="URI of the input stream")
parserd.add_argument("output_URI", type=str, default="", nargs='?', help="URI of the output stream")
parserd.add_argument("--network", type=str, default="fcn-mobilenet", help="pre-trained model to load, see below for options")
parserd.add_argument("--visualize", type=str, default="input,depth", help="visualization options (can be 'input' 'depth' 'input,depth'")
parserd.add_argument("--depth-size", type=float, default=1.0, help="scales the size of the depth map visualization, as a percentage of the input size (default is 1.0)")
parserd.add_argument("--filter-mode", type=str, default="linear", choices=["point", "linear"], help="filtering mode used during visualization, options are:\n 'point' or 'linear' (default: 'linear')")
parserd.add_argument("--colormap", type=str, default="viridis-inverted", help="colormap to use for visualization (default is 'viridis-inverted')",
choices=["inferno", "inferno-inverted", "magma", "magma-inverted", "parula", "parula-inverted",
"plasma", "plasma-inverted", "turbo", "turbo-inverted", "viridis", "viridis-inverted"])
# Initialize display type and parse arguments
is_headless = ["--headless"] if sys.argv[0].find('console.py') != -1 else [""]
try:
args = parser.parse_known_args()[0]
except:
print("")
parser.print_help()
sys.exit(0)
try:
argsd = parserd.parse_known_args()[0]
except:
print("")
parserd.print_help()
sys.exit(0)
# convert colorspace
def convert_color(img, output_format):
converted_img = cudaAllocMapped(
width=img.width, height=img.height, format=output_format)
cudaConvertColor(img, converted_img)
return converted_img
# Text definition
font = jetson.utils.cudaFont( size=20 )
# Generae Gaussian convolution filter
width, height=640, 360
# Develope filter regions
rows_per_region = width // 3
cols_per_region = height // 2
# Build array
x = np.linspace(0, width-1, width)
y = np.linspace(0, height-1, height)
z = np.linspace(0,2,3)
X, Y, Z = np.meshgrid(x, y, z)
# Define sigmas per axis
sigma_x=30
sigma_y=25
# Define Bar Arrangement
BarX=100
BarY=150
BarW=50
BarG=180
BarBX=120
BarBY=150
#define Normalized gassian function
def gaussian(x, y, z, mean, sigma_x, sigma_y):
exponent=-0.5 * ((x-mean[0])**2 / sigma_x**2 +(y - mean[1])**2 / sigma_y**2)
unormalized = np.exp(exponent)
return 254.0*(unormalized / np.max(unormalized))
# Bin Calculation
# regions = [(0, 210, 0, 180),(0, 210, 180, 360),(210, 420, 0, 180),(210, 420, 180, 360), (420, 630, 0, 180), (420, 630, 180, 360)]
regions = [(0, 180, 0, 210),(0, 180, 210, 420),(0, 180, 420, 630),(180, 360, 0 , 210), (180, 360, 210, 420), (180, 360, 420, 630)]
num_bins = 5
# Define gaussian filter
Gauss_filter_cuda = cudaImage(width=640, height=360, format='rgb32f')
Gauss_filter_nparray = np.zeros(Gauss_filter_cuda.shape, np.float32)
# Define depth mask
depth_mask_cuda = cudaImage(width=640, height=360, format='rgb32f')
Depth_mask640x360_array = np.zeros((360,640), np.float32)
Depth_mask640x360_array_1 = np.ones((360,640), np.float32)
# Mask = Mask*128
# Mask = np.zeros((360,640,3))
# create image filter
# num_rectangles = 6
rows = 3
cols = 2
diameter = 20
# percentage_smaller = 90
rect_width = 640 // rows# Rows
rect_height = 360 // cols # Cols
# create image filter
GFilter640x480_array = (gaussian(X , Y , Z, [width/6, height/4], sigma_x, sigma_y) +
gaussian(X , Y , Z, [width/2, height/4], sigma_x, sigma_y) +
gaussian(X , Y , Z, [5*width/6, height/4], sigma_x, sigma_y) +
gaussian(X , Y , Z, [width/6, 3*height/4], sigma_x, sigma_y) +
gaussian(X , Y , Z, [width/2, 3*height/4], sigma_x, sigma_y) +
gaussian(X , Y , Z, [5*width/6, 3*height/4], sigma_x, sigma_y))
# convert array to Image and map to rgb8 color space
GFilter640x480_cuda = cudaFromNumpy(GFilter640x480_array)
GFilter640x480_cuda_rgb8 = convert_color(GFilter640x480_cuda, "rgb8")
# create video sources and outputs
input = videoSource(args.input_URI, argv=sys.argv)
output = videoOutput(args.output_URI, argv=sys.argv+is_headless)
# load the object detection network
net = detectNet(args.network, sys.argv, args.threshold)
# Load depth network and set to obtain raw depth data
netd = depthNet(argsd.network, sys.argv)
depth_field = netd.GetDepthField()
Depth224x224_raw_array = jetson.utils.cudaToNumpy(depth_field)
buffers = depthBuffers(argsd)
# note: to hard-code the paths to load a model, the following API can be used:
#
# net = detectNet(model="model/ssd-mobilenet.onnx", labels="model/labels.txt",
# input_blob="input_0", output_cvg="scores", output_bbox="boxes",
# threshold=args.threshold)
# IP Socket Server running in a Thread
class MySocketServer:
def __init__(self, host, port):
self.server_socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server_socket.bind((host, port))
self.server_socket.listen(1)
self.server_socket, _ = self.server_socket.accept()
def receive_byte(self):
data = self.server_socket.recv(1).decode()
# print(data)
return data
def close(self):
# self.client_socket.close()
self.server_socket.close()
# State machine server
class MyStateMachine(StateMachine):
# State 0, Depth Mode(Default)
state_0 = State('0', initial=True)
# State 1, Object Detection Mode
state_1 = State('1')
# State 2, Calibration Mode
state_2 = State('2')
# Legal Transitions
# From 0 to 1, (From Navigate Mode to Detection Mode)
to_state_1 = state_0.to(state_1)
# From 2 to 0, (From Detection Mode to Depth Mode)
to_state_0 = state_2.to(state_0)
# From State 1 to State 0
to_state_10 = state_1.to(state_0)
# From 0 to 2, (From Navigate to Calibration)
to_state_02 = state_0.to(state_2)
# From 1 to 2, (From Detection Mode to Calibration)
to_state_2 = state_1.to(state_2)
# From 2 to 0, (From Calibration back to Navigate)
# State transition controller
def state_transition(server,state_machine):
global Detection
while True:
# Wait for socket byte command {0}Navigate,({1}Person,{2}Table),{3}Calibrate
Command = server.server_socket.recv(1).decode()
# Debug print
print(Command)
# If state is [1]Detection and a {0}command to [0]Navigate is requested, go ahead and do it
if (Command == '0') & state_machine.is_state_1:
state_machine.to_state_10()
# Navigate does not requiere object label (Does not apply)
Detection=-1
# if state is [1]Detection with a {1}command(person) then just change Detection Label=1
elif (Command == '1') & state_machine.is_state_1:
# No need to transtion
Detection=1 #(Person Label Index)
# if state is [1]Detection with a {2}command(table)) then just change Detection Label=67
elif (Command == '2') & state_machine.is_state_1:
# No Need to transition
Detection=67 #(Table label Index)
# if state is [0]Navigate with a new with label(1) then transtion to detection and change Detection Label
elif (Command == '1') & state_machine.is_state_0:
state_machine.to_state_1()
Detection=1 #(Person Label Index)
# if state is [0]Navigate with a new with label(1) then transtion to detection and change Detection Label
elif (Command == '2') & state_machine.is_state_0:
state_machine.to_state_1()
Detection=67 #(Table label Index)----------------------------------------------------
# if state is [0]Navigate and {3}Calibration is requested goto state [2]Calibration, no label requiered
elif (Command == '3') & state_machine.is_state_0:
state_machine.to_state_02()
Detection=-1 #Not used for calibration
# if state is [1]Detection and {3}Calibration is requested goto state [2]Calibration, no label requiered
elif (Command == '3') & state_machine.is_state_1:
state_machine.to_state_2()
Detection=-1 #No Label
# if state is [0]Calibration and {0}Navigation is requested goto state [0]Navigation, no label requiered
elif (Command == '0') & state_machine.is_state_2:
state_machine.to_state_0()
Detection=-1 #Not used for calibration
# Bisplay Thread, changes bibration level to all 6 Bixels
#[A][B][C]
#[C][D][E]
def SerialRead():
global dials
global M8p
while True:
data = M8p.readline().decode().strip()
numbers_str=data.split('\t')
dials=[254-int(num) for num in numbers_str]
def BivXplayer():
global BivXelA, BivXelB, BivXelC, BivXelD, BivXelE, BivXelF
while True:
#with variable_lock:
set_pwm(0,BivXelA) #BivXel1
set_pwm(1,BivXelB)#BivXel2
set_pwm(2,BivXelC) #BivXel3
set_pwm(11,BivXelD) #BivXel4
set_pwm(10,BivXelE) #BivXel5
set_pwm(9,BivXelF) #BivXel6
set_pwm(7,BivXelB) #test port
time.sleep(0.05)
#
# print(BivXelA, BivXelB, BivXelC, BivXelD, BivXelE, BivXelF)
#time.sleep(0.5)
# Sigmoid
# Colculate Distance activation based on the sigmoid function and inverting the result. This is multiplied by scale factor
# its result will send a Vibration value to ecach BivXel as a function inversly proportional to the Distance
# The closer the object th highr the vibration, but activated only after a threshold is passed
# The 'mode' boolean controls positive or negative activation
# The 'slope' controls how fas the activation respondes as a change in depth
def SigmoidF(x, ScaleV, ActivationV, slope, mode):
if mode:
return ScaleV*(1.0 - 1.0/(1.0 +np.exp(- slope*(x-ActivationV))))
else:
return ScaleV*(1.0/(1.0 +np.exp(- slope*(x-ActivationV))))
# Quantizased steps
# Returns a speciified number of steps that steps the output ti a predtermined number if vaules
def quantize_output(y, segments, scale):
step_size= scale/segments
return np.round(y/step_size) * step_size
def main():
global BivXelA, BivXelB, BivXelC, BivXelD, BivXelE, BivXelF
BivXelA, BivXelB, BivXelC, BivXelD, BivXelE, BivXelF = 0, 0, 0, 0, 0, 0
global Detection
Detection=-1
global dials
dials = [0,0,0,0,0,0,0,0]
# Check if USB Serila Calibration Dials are present, if yes open port and start thread
# if not just read calibration file from disk
BivXplayer_thread = threading.Thread(target=BivXplayer)
BivXplayer_thread.start()
print('Print Thread Stared')
print("Let the games begin...")
host = socket.gethostname() # as both code is running on same pc
port = 12453 # socket server port number ********************************************************************************
server = MySocketServer(host,port)
print('Socket Sever Stared')
state_machine = MyStateMachine()
print('State Machine Started')
transition_thread = threading.Thread(target=state_transition, args=(server, state_machine))
transition_thread.start()
print('Transition Thread Started')
global M8p
# Start Serail Thread if port found, otherwise read file
try:
M8p = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
print('M8P Calibration dials found')
Cal='M8P'
serial_thread = threading.Thread(target=SerialRead)
serial_thread.start()
print('Serial Thread Started')
except serial.SerialException:
print("M8P not found, using stored configuration file")
dials=np.load('CALIBRATION.npy')
Cal='CALIBRATION.npy'
CurrState='Null'
# Main Loop,
try:
while True:
# discrimanate between two states(Depth[0] and Detection[1])
# MonoDepth
if state_machine.is_state_0:
# State-0 (Navigation)
CurrState='Nav'
# Get Image from camera GFilter640x480_cuda [Camera1920x720_cuda]
Camera = input.Capture()
#Allocate buffers from Camera info
buffers.Alloc(Camera.shape, Camera.format)
# Create buffer for resized image, same format
Camera640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
# Resize camera img to resized_img
jetson.utils.cudaResize(Camera, Camera640x360_cuda_rgb8)
# Clear Depth Mask Array with 0.0s, only let detpth values through if object detected
Depth_mask640x360_array = np.zeros(Camera640x360_cuda_rgb8.shape, np.float32)
# Process depth from image without overlays
netd.Process(Camera, buffers.depth, argsd.colormap, argsd.filter_mode)
# Create Display Buffers
Bivxel640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
Depth640x360_raw_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
Detect640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
Bar640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
Diag640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)
# resize camera detections+object overlays into buffer
jetson.utils.cudaResize(Camera, Detect640x360_cuda_rgb8)
# Resize mono depth to 640 by 360
Depth224x224_raw_cuda = cudaFromNumpy(50*Depth224x224_raw_array)
Depth224x224_raw_cuda_rgb8 = convert_color(Depth224x224_raw_cuda, "rgb8")
jetson.utils.cudaResize(Depth224x224_raw_cuda_rgb8, Depth640x360_raw_cuda_rgb8)
# Extract numpy array from de resized depth image
Depth640x360_raw_array = cudaToNumpy(Depth640x360_raw_cuda_rgb8)
# Get Gaussion filtered depth, Multiply Resized Depth numpy array times Numpy GFilter and ccolor convert to RGB8
DepthxGFilter640x360_array = np.multiply( Depth640x360_raw_array/254 ,GFilter640x480_array)
DepthxGFilter640x36_cuda = cudaFromNumpy(DepthxGFilter640x360_array)
DepthxGFilter640x36_cuda_rgb8 = convert_color(DepthxGFilter640x36_cuda, "rgb8")
# Depth is not masked in navigation
DepthxDmask640x360_array = Depth640x360_raw_array
DepthxDmask640x360_cuda = cudaFromNumpy(DepthxDmask640x360_array) # Convert arra to cuda
DepthxDmask640x360_cuda_rgb8 = convert_color(DepthxDmask640x360_cuda, "rgb8") # Map to color space
# Calculate depth image average after Gaussin filter applied
averages = np.zeros(6)
for i in range(6):
Dregion=DepthxGFilter640x360_array[regions[i][0]:regions[i][1]+1,regions[i][2]:regions[i][3]+1 ]
averages[i]= np.mean(Dregion)
# Distance corection factor (Feet to meters 3.03???)
Feet2Meter=1.0 #0.1525
# Display numeric averages in [E] sector (VibXel)
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[0]*Feet2Meter)), 110, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[1]*Feet2Meter)), 290, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[2]*Feet2Meter)), 470, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[3]*Feet2Meter)), 110, 310, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[4]*Feet2Meter)), 290, 310, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[5]*Feet2Meter)), 470, 310, (150, 150, 150), (0, 0, 0))
# Draw Bars on screen that represnt the average distance per region
BarDistScale=3
for j in range(3):
jetson.utils.cudaDrawRect(Bar640x360_cuda_rgb8, (BarX+BarG*j,BarY-int(averages[j])*BarDistScale, BarX+BarG*j+BarW,BarY+1), (127,255,200,200)) #(0, 0, 210, 180)
for j in range(3,6):
jetson.utils.cudaDrawRect(Bar640x360_cuda_rgb8, (BarX+BarG*(j-3),BarBY+BarY-int(averages[j])*BarDistScale, BarX+BarG*(j-3)+BarW,BarBY+BarY+1), (127,255,200,200)) #(0, 0, 210, 180)
# Calculate inverse BivXel using sigmoid distance activation function(Signal user that an object is near, the closer the obhect the higher the bivration)
Sigmoid=np.zeros(0)
# define Activation parameters
ScaleNav = dials[0]*8 # 4096 is max valu, set to 1/2 for testing purposes
ScaleDet = dials[4]*8
# Nav Activation Distance
ActivationNav = dials[1]/10 # (Feet/Meters)Distance where activation vale will activate
ActivationDet = dials[5]/10
# Sigmoid Slope
SlopeNav = dials[2]/10
SlopeDet = dials[6]/10
# motor bias
BiasNav = dials[3]*8
BiasDet = dials[7]*8
# Define BivXel calibration parameter, calibrates each individual vibrator to "feel" the same mechanicaly/physiologically
BivXelCal = np.ones(6)
BivXelCal[0]=1.0
BivXelCal[1]=1.0
BivXelCal[2]=1.0
BivXelCal[3]=1.0
BivXelCal[4]=1.0
BivXelCal[5]=1.0
# Calculate activation
Sigmoid = SigmoidF(averages,ScaleNav,ActivationNav,SlopeNav,True) #
# Sigmoid = averages
# Scale each VibXel accordingly
SigmoidCal = np.multiply(Sigmoid, BivXelCal)
# Dislay Bar bibxels wich is inverse if the distance, Bar value has a proprtional constant of 3 for space limitation
BarBivScale=2
for j in range(3):
jetson.utils.cudaDrawRect(Bivxel640x360_cuda_rgb8, (BarX+BarG*j,BarY-int(SigmoidCal[j]*BarBivScale), BarX+BarG*j+BarW+1,BarY+1), (127,127,250,200)) #(0, 0, 210, 180)
for j in range(3,6):
jetson.utils.cudaDrawRect(Bivxel640x360_cuda_rgb8, (BarX+BarG*(j-3),BarBY+BarY-int(SigmoidCal[j]*BarBivScale), BarX+BarG*(j-3)+BarW+1,BarBY+BarY+2), (127,127,250,200))
# Display on BivXel screen
#with variable_lock:
BivXelA= int(SigmoidCal[0]) + BiasNav
BivXelB= int(SigmoidCal[1]) + BiasNav
BivXelC= int(SigmoidCal[2]) + BiasNav
BivXelD= int(SigmoidCal[3]) + BiasNav
BivXelE= int(SigmoidCal[4]) + BiasNav
BivXelF= int(SigmoidCal[5]) + BiasNav
# Display VibXel Text , window [E]
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelA), 110, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelB), 290, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelC), 470, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelD), 110, 310, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelE), 290, 310, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelF), 470, 310, (150, 0, 150), (0, 0, 0))
# Calibartion Text in Diagnostics window [D]
array=np.array(dials)
YL1=100
YL2=200
YL3=300
X0=150
XG=140
T0=50
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Scale:', T0, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ScaleNav), X0, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Activ:', T0+XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ActivationNav), X0+XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Slope:', T0+2*XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(SlopeNav), X0+2*XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Nav Bias:', T0+3*XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(BiasNav), X0+3*XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Scale:', T0, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ScaleDet), X0, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Activ:', T0+XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ActivationDet), X0+XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Slope:', T0+2*XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(SlopeDet), X0+2*XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Det Bias:', T0+3*XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(BiasDet), X0+3*XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'State Num:', T0, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, CurrState, X0, YL3, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Calibrat :', T0+XG, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, Cal, X0+XG, YL3, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Object:', T0+3*XG, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(Detection), X0+3*XG, YL3, (0, 150, 0), (0, 0, 0))
# Display Arrangement
# [A][B][C]
# [D][E][F]
#
# Display images in overlay
# [A] Image + Detection overlay
cudaOverlay(Detect640x360_cuda_rgb8, buffers.composite, 0, 0) # (1,1) Camera and Detection
# [B] Depth * DMask(Function of state, S0>Mask=1.0[Pass all], S1>Mask as function of tection[Pass detected detpth ony])
cudaOverlay(DepthxDmask640x360_cuda_rgb8, buffers.composite, 640, 0) # (1,2) Depth Mask resized_bisplay
# [C] Gaussion Filter Mapp Reference
cudaOverlay(DepthxGFilter640x36_cuda_rgb8, buffers.composite, 1280,0) #(1,3) Gaussian Filter
# [D] Depth*GMask*DMask (DMask fubction of state)
cudaOverlay(Diag640x360_cuda_rgb8 , buffers.composite,0, 360) # [2,1] Diagnostics/Status
# [E] Bibxels,
cudaOverlay(Bivxel640x360_cuda_rgb8, buffers.composite, 640, 360) # [2,2] 9Circles)
# [F] Depth Bars
cudaOverlay(Bar640x360_cuda_rgb8, buffers.composite,1280, 360) # [2,3] Gaussioan Filtered
# render the image
output.Render(buffers.composite)
# update the title bar
output.SetStatus("{:s} | Network {:.0f} FPS".format(args.network, net.GetNetworkFPS()))
# print out performance info
net.PrintProfilerTimes()
# Object Detection ------------------------------------------------------------------------------------------------------
elif state_machine.is_state_1:
# State-1 (Object Detection)
# Get Image from camera GFilter640x480_cuda [Camera1920x720_cuda]
Camera = input.Capture()
CurrState='Det'
#Allocate buffers from Camera info
buffers.Alloc(Camera.shape, Camera.format)
# Create buffer for resized image, same format
Camera640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)#[A]
# Resize camera img to resized_img
jetson.utils.cudaResize(Camera, Camera640x360_cuda_rgb8)
# Clear Depth Mask Array with 0.0s, only let detpth values through if object detected
Depth_mask640x360_array = np.zeros(Camera640x360_cuda_rgb8.shape, np.float32)
# Process depth from image without overlays
netd.Process(Camera, buffers.depth, argsd.colormap, argsd.filter_mode)
# Create Display Buffers
Bivxel640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format) #[E]
Depth640x360_raw_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)#[B]
Detect640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)#[B]
Bar640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)#[E]
Diag640x360_cuda_rgb8 = jetson.utils.cudaAllocMapped(width=640,height=360,format=Camera.format)#[D]
# Find detections in image and resize with overlay detections
detections = net.Detect(Camera, overlay=args.overlay)
# resize camera detections+object overlays into buffer
jetson.utils.cudaResize(Camera, Detect640x360_cuda_rgb8)
# Loop for a sibgle object (ignore if more that one object of the same lable is found)
for detection in detections:
#print(detection)
if detection.ClassID == Detection:
x1=int(detection.Left//2)
y1=int(detection.Right//2)
x2=int(detection.Top//2)
y2=int(detection.Bottom//2)
#print(x1,x2,y1,y2)
# Fill the depth mask when and object is found
Depth_mask640x360_array[x2:y2,x1:y1]=1.0
# Inform Bisplay of detction label
# with variable_lock:
# shared_variable=detection.ClassID
# (0, 210, 0, 180)
# >>>jetson.utils.cudaDrawRect(Bar640x360_cuda_rgb8, (0, 0, 210, 180), (127,255,200,200))
# Get Raw depth from array pointer
Depth224x224_raw_cuda = cudaFromNumpy(50*Depth224x224_raw_array)
Depth224x224_raw_cuda_rgb8 = convert_color(Depth224x224_raw_cuda, "rgb8")
jetson.utils.cudaResize(Depth224x224_raw_cuda_rgb8, Depth640x360_raw_cuda_rgb8)
# Extract numpy array from de resized image
Depth640x360_raw_array = cudaToNumpy(Depth640x360_raw_cuda_rgb8)
# Multiply bt gaussian (Filter)
DepthxDmask640x360_array = np.multiply( Depth_mask640x360_array ,Depth640x360_raw_array)
DepthxDmask640x360_cuda = cudaFromNumpy(DepthxDmask640x360_array) # Convert arra to cuda
DepthxDmask640x360_cuda_rgb8 = convert_color(DepthxDmask640x360_cuda, "rgb8") # Map to color space
# Get Gaussion filtered depth, Multiply Resized Depth numpy array times Numpy GFilter and ccolor convert to RGB8
DepthxGFilter640x360_array = np.multiply( Depth640x360_raw_array/254 ,GFilter640x480_array)
DepthxGFilter640x36_cuda = cudaFromNumpy(DepthxGFilter640x360_array)
DepthxGFilter640x36_cuda_rgb8 = convert_color(DepthxGFilter640x36_cuda, "rgb8")
DepthxGFilterxDMask640x360_array = np.multiply( DepthxGFilter640x360_array, Depth_mask640x360_array)
DepthxGFilterxDmask640x36_cuda = cudaFromNumpy(DepthxGFilterxDMask640x360_array)
DepthxGFilterxDmask640x36_cuda_rgb8 = convert_color(DepthxGFilterxDmask640x36_cuda, "rgb8")
averages = np.zeros(6)
# Calculate average per region from Object detected mask with gausiona filter
for i in range(6):
#Dregion=DepthxDmask640x360_array[regions[i][0]:regions[i][1]+1,regions[i][2]:regions[i][3]+1 ]
Dregion=DepthxGFilterxDMask640x360_array[regions[i][0]:regions[i][1]+1,regions[i][2]:regions[i][3]+1 ]
averages[i]= np.mean(Dregion)
# Display Distance Mask by objcect average [F]
Feet2Meter=1.0 #0.1525
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[0]*Feet2Meter)), 110, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[1]*Feet2Meter)), 290, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[2]*Feet2Meter)), 470, 160, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[3]*Feet2Meter)), 110, 310, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[4]*Feet2Meter)), 290, 310, (150, 150, 150), (0, 0, 0))
font.OverlayText(Bar640x360_cuda_rgb8, 640, 360, str(int(averages[5]*Feet2Meter)), 470, 310, (150, 150, 150), (0, 0, 0))
# Draw Bars on screen that represnt the average distance per region
BarDistScale = 3
for j in range(3):
jetson.utils.cudaDrawRect(Bar640x360_cuda_rgb8, (BarX+BarG*j,BarY-int(averages[j])*BarDistScale, BarX+BarG*j+BarW,BarY+1), (127,255,200,200)) #(0, 0, 210, 180)
for j in range(3,6):
jetson.utils.cudaDrawRect(Bar640x360_cuda_rgb8, (BarX+BarG*(j-3),BarBY+BarY-int(averages[j])*BarDistScale, BarX+BarG*(j-3)+BarW,BarBY+BarY+1), (127,255,200,200)) #(0, 0, 210, 180)
# Calculate inverse BivXel using sigmoid distance activation function(Signal user that an object is near, the closer the obhect the higher the bivration)
Sigmoid=np.zeros(0)
# define Activation parameters
ScaleDet = dials[4]*8 # 4096 # PWM max value
ScaleNav = dials[0]*8
# Nav Activation Distance
ActivationDet = dials[5]/10 # (Feet/Meters)Distance where activation vale will activate
ActivationNav = dials[1]/10
# Sigmoid Slope
SlopeDet = dials[6]/10
SlopeNav = dials[2]/10
# motor bias
BiasNav = dials[3]*8
BiasDet = dials[7]*8
# Define BivXel calibration parameter, calibrates each individual vibrator to "feel" the same mechanicaly/physiologically
BivXelCal = np.ones(6)
BivXelCal[0]=1.0
BivXelCal[1]=1.0
BivXelCal[2]=1.0
BivXelCal[3]=1.0
BivXelCal[4]=1.0
BivXelCal[5]=1.0
# Calculate activation
Sigmoid = SigmoidF(averages,ScaleDet,ActivationDet,SlopeDet,False)
print(Sigmoid)
# Scale each VibXel accordingly
SigmoidCal = np.multiply(Sigmoid, BivXelCal)
#SigmoidCal = averages*dials[1]
#print(Sigmoid)
# Dislay Bar bibxels wich is inverse if the distance, Bar value has a proprtional constant of 3 for space limitation
BarBivScale=4
for j in range(3):
jetson.utils.cudaDrawRect(Bivxel640x360_cuda_rgb8, (BarX+BarG*j,BarY-int(SigmoidCal[j]*BarBivScale+1), BarX+BarG*j+BarW,BarY+1), (127,127,250,200)) #(0, 0, 210, 180)
for j in range(3,6):
jetson.utils.cudaDrawRect(Bivxel640x360_cuda_rgb8, (BarX+BarG*(j-3),BarBY+BarY-int(SigmoidCal[j]*BarBivScale+1), BarX+BarG*(j-3)+BarW,BarBY+BarY+1), (127,127,250,200))
# Display on BivXel screen if not 0, avoid singularities
#with variable_lock:
BivXelA= int(SigmoidCal[0]) + BiasDet
BivXelB= int(SigmoidCal[1]) + BiasDet
BivXelC= int(SigmoidCal[2]) + BiasDet
BivXelD= int(SigmoidCal[3]) + BiasDet
BivXelE= int(SigmoidCal[4]) + BiasDet
BivXelF= int(SigmoidCal[5]) + BiasDet
# Display VibXel [E]
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelA), 110, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelB), 290, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelC), 470, 160, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelD), 110, 310, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelE), 290, 310, (150, 0, 150), (0, 0, 0))
font.OverlayText(Bivxel640x360_cuda_rgb8, 640, 360, str(BivXelF), 470, 310, (150, 0, 150), (0, 0, 0))
# Calibartion Text in Diagnostics window [D]
array=np.array(dials)
YL1=100
YL2=200
YL3=300
X0=150
XG=140
T0=50
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Scale:', T0, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ScaleNav), X0, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Activ:', T0+XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ActivationNav), X0+XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Nav Slope:', T0+2*XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(SlopeNav), X0+2*XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Nav Bias:', T0+3*XG, YL1, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(array[3]), X0+3*XG, YL1, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Scale:', T0, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ScaleDet), X0, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Activ:', T0+XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(ActivationDet), X0+XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Det Slope:', T0+2*XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(SlopeDet), X0+2*XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Det Bias:', T0+3*XG, YL2, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(array[7]), X0+3*XG, YL2, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'State Num:', T0, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, CurrState, X0, YL3, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, 'Calibrat :', T0+XG, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, Cal, X0+XG, YL3, (0, 150, 0), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, ' Object:', T0+3*XG, YL3, (150, 150, 150), (0, 0, 0))
font.OverlayText(Diag640x360_cuda_rgb8, 640, 360, str(Detection), X0+3*XG, YL3, (0, 150, 0), (0, 0, 0))
# Display Arrangement
# [A][B][C]
# [D][E][F]
#
# Display images in overlay
# [A] Image + Detection overlay
cudaOverlay(Detect640x360_cuda_rgb8, buffers.composite, 0, 0) # (1,1) Camera and Detection
# [B] Depth * DMask(Function of state, S0>Mask=1.0[Pass all], S1>Mask as function of tection[Pass detected detpth ony])
cudaOverlay(DepthxDmask640x360_cuda_rgb8, buffers.composite, 640, 0) # (1,2) Depth Mask resized_bisplay
# [C] Depth*GFilter*DetectMask
cudaOverlay(DepthxGFilterxDmask640x36_cuda_rgb8 , buffers.composite, 1280,0) #(1,3) Gaussian Filter
#cudaOverlay(DepthxDmask640x360_cuda_rgb8 , buffers.composite, 1280,0) #(1,3) Gaussian Filter
# [D] Depth*GMask*DMask (DMask fubction of state)
cudaOverlay(Diag640x360_cuda_rgb8 , buffers.composite,0, 360) # [2,1] Diagnostics/Status
# [E] Bibxels,
cudaOverlay(Bivxel640x360_cuda_rgb8, buffers.composite, 640, 360) # [2,2] 9Circles)
# [F] Histograms
cudaOverlay(Bar640x360_cuda_rgb8, buffers.composite,1280, 360) # [2,3] Gaussioan Filtered
# render the image
output.Render(buffers.composite)
# update the title bar
output.SetStatus("{:s} | Network {:.0f} FPS".format(args.network, net.GetNetworkFPS()))
# print out performance info
net.PrintProfilerTimes()
# cALIBRATION ------------------------------------------------------------------------------------------------------
elif state_machine.is_state_2:
CurrState='Cal'
# Store calibartion array end exit to [0]Navigation state
np.save('CALIBRATION.npy',dials)
# From 2 to 0, (From Detection Mode to Depth Mode)
state_machine.to_state_0()
# try:
# data = M8p.readline().decode().strip()
# numbers_str=data.split('\t')
# dials=[254-int(num) for num in numbers_str]
# array=np.array(dials)
# if data:
print(dials)
# # Display Diagnostics
# except:
# M8p.close()
# exit on input/output EOS
if not input.IsStreaming() or not output.IsStreaming():
break
except KeyboardInterrupt:
pass
finally:
# server.close()
print('Closing Server')
if __name__ == "__main__":
main()
Comments