def __init__(self, step_pin, dir_pin, mode_pins, step_type):
"""docstring for ."""
self.step_pin = step_pin
self.dir_pin = dir_pin
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(step_pin, GPIO.OUT)
GPIO.setup(dir_pin, GPIO.OUT)
GPIO.setup(mode_pins, GPIO.OUT)
resolution = {
'Full': (0, 0),
'Half': (1, 0),
'1/4': (0, 0), # M0 must be disconnected
'1/8': (0, 1),
'1/16': (1, 1),
'1/32': (0, 1)
} # M0 must be disconnected
microsteps = {
'Full': 1,
'Half': 2,
'1/4': 4,
'1/8': 8,
'1/16': 16,
'1/32': 32
}
self.delay = .005 / microsteps[step_type]
GPIO.output(mode_pins, resolution[step_type])
def main():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
# GPIO.setup([stop, rec, menu], GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup([stop, rec, menu], GPIO.IN)
# # attach callbacks to events
# GPIO.add_event_detect(stop, GPIO.RISING, callback=stop_cb, bouncetime=200)
# GPIO.add_event_detect(rec, GPIO.RISING, callback=rec_cb, bouncetime=200)
# GPIO.add_event_detect(menu, GPIO.RISING, callback=menu_cb, bouncetime=200)
GPIO.add_event_detect(stop, GPIO.RISING)
GPIO.add_event_detect(rec, GPIO.RISING)
GPIO.add_event_detect(menu, GPIO.RISING)
while True:
try:
if GPIO.event_detected(stop):
stop_cb()
if GPIO.event_detected(rec):
rec_cb()
if GPIO.event_detected(menu):
menu_cb()
pass
except KeyboardInterrupt:
GPIO.cleanup()
sys.exit()
def __init__(self):
super().__init__('status_light')
self.sample_received = False
self.light_on = False
# Init GPIO
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(7, GPIO.OUT, initial=GPIO.LOW)
GPIO.output(7, GPIO.LOW)
# Create QOS for subscriber
qos_klr_v = QoSProfile(history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
# Create subscriber
self.sub = self.create_subscription( msg_type=UInt32,
topic='camera_trigger',
callback=self.listener_callback,
qos_profile=qos_klr_v )
# Setup timer
timer_period_secs = 0.2
self.timer = self.create_timer( timer_period_secs, self.timer_callback )
def __init__(self, trigpin=None, echopin=None):
self.__trigpin = trigpin
self.__echopin = echopin
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(trigpin, GPIO.OUT)
GPIO.setup(echopin, GPIO.IN)
def __init__(self, out1, out2, pca9685, pwm):
self.__out1 = out1
self.__out2 = out2
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(out1, GPIO.OUT, initial= GPIO.LOW)
GPIO.setup(out2, GPIO.OUT, initial=GPIO.LOW)
self.__pca9685 = pca9685
self.__pwm = pwm
def __init__(self, TOP, MID, BOT):
super(StepMotor, self).__init__()
print('[ Info ] Step Motor Initialized Start')
self.channels = [TOP, MID, BOT]
self.state = [0, 0, 0]
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(self.channels, GPIO.OUT, initial=GPIO.LOW)
def power_on(power_key):
print('SIM7600X is starting:')
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(power_key, GPIO.OUT)
time.sleep(0.1)
GPIO.output(power_key, GPIO.HIGH)
time.sleep(2)
GPIO.output(power_key, GPIO.LOW)
time.sleep(20)
ser.flushInput()
print('SIM7600X is ready')
def alarm():
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(True)
GPIO.setup(23, GPIO.OUT)
try:
print("LED on")
GPIO.output(23, GPIO.HIGH)
time.sleep(3)
#print ("LED off")
GPIO.output(23, GPIO.LOW)
GPIO.cleanup()
except:
KeyboardInterrupt
def __init__(self, redpin=None, greenpin=None, bluepin=None):
self.__redpin = redpin
self.__greenpin = greenpin
self.__bluepin = bluepin
self.state = None
# GPIO 설정
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
if redpin is not None:
GPIO.setup(redpin, GPIO.OUT, initial=GPIO.HIGH)
if greenpin is not None:
GPIO.setup(greenpin, GPIO.OUT, initial=GPIO.HIGH)
if bluepin is not None:
GPIO.setup(bluepin, GPIO.OUT, initial=GPIO.HIGH)
def __init__(self):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(DC_ENA, GPIO.OUT)
GPIO.setup(DC_IN1, GPIO.OUT)
GPIO.setup(DC_IN2, GPIO.OUT)
GPIO.setup(SERVO, GPIO.OUT)
GPIO.setup(HEADLIGHT, GPIO.OUT)
GPIO.setup(BACKLIGHT, GPIO.OUT)
GPIO.setup(BREAKLIGHT, GPIO.OUT)
GPIO.setup(WAVE_TRIG, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(WAVE_ECHO, GPIO.IN)
self.servo = GPIO.PWM(SERVO, 50)
self.servo.start(8)
def auto_controller():
global testing
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(7, GPIO.OUT)
''' Run Autonomous Controller Node '''
SM = AutonomousStateMachine() # Initialise state machine class
#ROS Publisher Subscribers and Services
server = rospy.Service('/core_rover/start_auto', StartAuto,
SM.handleStartAuto)
status_pub = rospy.Publisher("/core_rover/auto_status",
AutoStatus,
queue_size=10)
rospy.init_node('auto_controller', anonymous=True) # Initialise Node
rate = rospy.Rate(2) # Loop rate in Hz
rospy.loginfo("Autonomous Controller Started")
## testing
if testing:
time.sleep(2)
SM.des_pos = WaypointClass(-37.6617819, 145.3692175)
SM.toTraverse()
SM.toSearch()
## end testing
while not rospy.is_shutdown():
if getMode() == 'Auto' or testing:
SM.run()
rospy.loginfo('RUN ITER')
elif getMode() == "Standby":
drive_msg = DriveCmd()
drive_msg.rpm = 0
drive_msg.steer_pct = 0
SM.drive_pub.publish(drive_msg)
# TODO adjust rate that AutoStatus is published
status_msg = AutoStatus()
status_msg.auto_state = getAutonomousMode()
status_msg.latitude = SM.rovey_pos.latitude
status_msg.longitude = SM.rovey_pos.longitude
status_msg.bearing = SM.orientation
status_pub.publish(status_msg)
rate.sleep() # Sleep until next iteration
GPIO.cleanup(7)
def __init__(self, En_PinIn, Dir_PinIn, Step_PinIn, busIn=0, deviceIn=0):
# Import pin and bus setup from input
self.En_Pin = En_PinIn
self.Dir_Pin = Dir_PinIn
self.Step_Pin = Step_PinIn
self.bus = busIn
self.device = deviceIn
#Define the register address
self.WriteFlag = (1<<7)
self.ReadFlag = (0<<7)
self.Reg_GCONF = 0x00
self.Reg_GSTAT = 0x01
self.Reg_IOIN = 0x04
self.Reg_IHOLD_IRUN = 0x10
self.Reg_TPOWERDOWN = 0x11
self.Reg_TSTEP = 0x12
self.Reg_TRWMTHRS = 0x13
self.Reg_TCOOLTHRS = 0x14
self.Reg_THIGH = 0x15
self.Reg_XDIRECT = 0x2D
self.Reg_VDCMIN = 0x33
self.Reg_CHOPCONF = 0x6C
self.Reg_COOLCONF = 0x6D
self.Reg_DRVSTAT = 0x6F
self.Reg_PWMCONF = 0x70
self.Reg_ENCM_CTRL = 0x72
#Implement and set up he spi device object
self.spi = spidev.SpiDev()
self.spi.open(self.bus, self.device) # SPI2---->MOSI:37pin (/dev/SPI1.1)
self.spi.max_speed_hz = 1000000
self.spi.mode = 0b11
self.spi.lsbfirst = False
#Setup GPIO configuration
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(self.En_Pin, GPIO.OUT, initial=GPIO.HIGH) #LOW -> active
GPIO.setup(self.Dir_Pin, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(self.Step_Pin, GPIO.OUT, initial=GPIO.LOW)
def controle():
global estado_led
rospy.init_node('controle_led', anonymous=False, disable_signals=False)
rospy.loginfo("Iniciando no de controle do LED ...")
# Iniciando canal de saida GPIO da Jetson
channel = 13 # pino de saida que funcionou na Jetson
gpio.setmode(gpio.BOARD) # Usando numeracao raiz da placa
gpio.setwarnings(False) # parar com chatices
gpio.setup(channel, gpio.OUT) # vai funcionar como saida
# Servidor para controle do LED
l = rospy.Service('/controle_led', LED, callbackLED)
taxa = 10
rospy.loginfo("Rodando controle do LED a %d Hz...", taxa)
r = rospy.Rate(taxa)
while not rospy.is_shutdown():
# Variar segundo comando do LED
if estado_led == 1: # continuo
gpio.output(channel, gpio.HIGH)
gpio.output(channel, gpio.HIGH)
gpio.output(channel, gpio.HIGH)
if estado_led == 2: # pisca rapido
gpio.output(channel, gpio.HIGH)
sleep(0.3)
gpio.output(channel, gpio.LOW)
sleep(0.3)
gpio.output(channel, gpio.LOW)
if estado_led == 3: # pisca lento
gpio.output(channel, gpio.HIGH)
sleep(1)
gpio.output(channel, gpio.LOW)
sleep(1)
gpio.output(channel, gpio.LOW)
# Rodar o ciclo ros
r.sleep()
rospy.spin()
rospy.loginfo("Finalizando no ...")
def module_init():
# print("module_init")
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
# print("RST_PIN")
GPIO.setup(RST_PIN, GPIO.OUT)
# print("DC_PIN")
GPIO.setup(DC_PIN, GPIO.OUT)
# print("CS_PIN")
GPIO.setup(CS_PIN, GPIO.OUT)
# print("BUSY_PIN")
GPIO.setup(BUSY_PIN, GPIO.IN)
# SPI.max_speed_hz = 2000000
# SPI.mode = 0b00
# gpio2.SYSFS_GPIO_Export(15)
# gpio2.SYSFS_GPIO_Direction(15, 0)
spi.SYSFS_software_spi_begin()
return 0
def show():
"""Output the buffer to Blinkt!."""
global _gpio_setup
if not _gpio_setup:
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(DAT, GPIO.OUT)
GPIO.setup(CLK, GPIO.OUT)
atexit.register(_exit)
_gpio_setup = True
_sof()
for pixel in pixels:
r, g, b, brightness = pixel
_write_byte(0b11100000 | brightness)
_write_byte(b)
_write_byte(g)
_write_byte(r)
_eof()
class Pwm(SingletonConfigurable):
pin_num = 37
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(pin_num, GPIO.OUT, initial=GPIO.LOW)
turn_left = 0.00085 #min value
turn_middle = 0.00125
turn_right = 0.00165 #max value
def switch(self, high_time, angle=40):
for i in range(angle):
GPIO.output(self.pin_num, GPIO.HIGH)
time.sleep(high_time)
GPIO.output(self.pin_num, GPIO.LOW)
time.sleep(0.020 - high_time)
def gpio_set():
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32
from sys import argv
import time
import Jetson.GPIO as GPIO
import robot_dimensions as robo
'''
pinsRightEncoder = [24, 22]
pinsLeftEncoder = [16, 18]
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(pinsRightEncoder, GPIO.IN)
GPIO.setup(pinsLeftEncoder, GPIO.IN)
'''
def init(pins, counter):
print("Init")
if GPIO.input(pins[0]) == GPIO.HIGH and GPIO.input(pins[1]) == GPIO.HIGH:
case = 2
else:
case = 1
return case, counter
def wait(pins, counter):
print("wait")
while True:
if GPIO.input(pins[0]) == GPIO.HIGH and GPIO.input(
pins[1]) == GPIO.HIGH:
def init_rpi(): GPIO.setmode(GPIO.BOARD) GPIO.setwarnings(False) GPIO.setup(controlPin, GPIO.OUT, initial = GPIO.LOW)
def __init__(self, channel):
self.__channel = channel
self.state = Buzzer.OFF
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(channel, GPIO.OUT, initial=GPIO.HIGH)
def listener():
rospy.init_node('arm', anonymous=False)
rospy.set_param('base_station/drive_mode', 'XboxDrive')
rospy.Subscriber("/base_station/rjs_raw_ctrl", RawCtrl, RightCallback)
rospy.Subscriber("/base_station/ljs_raw_ctrl", RawCtrl, LeftCallback)
rospy.Subscriber("/heartbeat", Empty, HBeatCb)
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(31, GPIO.OUT)
while (True):
global finger_debouncer
global toggle_finger
global hbeat_cnt
global ignore_endstops
global reset
global values
global prev_finger
rospy.loginfo(ignore_endstops)
hbeat_cnt += 1
if False and ((hbeat_cnt > max_hbeat) or (reset == True)):
hbeat = False
values = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for i in range(0, len(values)): #Sending stop commands to all
field = 0x0
complete_id = (
ids[i] << 4) + field #Embedding ID's with 0 command
msg = can.Message(
arbitration_id=complete_id, data=[],
extended_id=False) #Creating can message to send
bus.send(msg)
else:
for i in range(0, len(values)):
if (i == 4
or i == 6) and (ignore_endstops
== True): #If on ignore endstops mode
field = 0x5
if values[i] < 0:
field = 0x6
value = int(abs(values[i]) * 4095)
else: #If not on ignore end_stops mode or not an endeffector one
field = 0x3
if values[i] < 0:
field = 0x4
value = int(abs(values[i]) * 4095)
if (i == 6):
if toggle_finger == True:
#field = 0x7
toggle_finger = False #Finger toggle debouncer
if prev_finger == True:
GPIO.output(31, GPIO.LOW)
rospy.loginfo("Off")
prev_finger = False
else:
GPIO.output(31, GPIO.HIGH)
prev_finger = True
rospy.loginfo("On")
complete_id = (
ids[i] << 4
) + field #Embedding ID's with chosen directional PWM command
if value < 10:
value = 0 #Getting rid of off centre
bit1 = value >> 8 & 0xFF
bit2 = value & 0xFF
#rospy.loginfo(value)
msg = can.Message(arbitration_id=complete_id,
data=[bit1, bit2],
extended_id=False)
bus.send(msg)
if finger_debouncer < 15:
finger_debouncer += 1
time.sleep(0.1)
def _set_operating_params(self):
'''
Sets all the operating parameters for control of PUFFER, Pinouts, and
safety parameters.
returns None
'''
#GPIO settings for E-stop and Regulator Enable pins
self.e_stop_pin = rospy.get_param("/gpio_pins/e_stop",1)
self.reg_en_pin = rospy.get_param("/gpio_pins/reg_en",1)
try:
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(self.e_stop_pin, GPIO.OUT)
GPIO.setup(self.reg_en_pin, GPIO.OUT)
except:
pass
#Threadlock used for serial comm to RoboClaw
self.thread_timeout = rospy.get_param("/threadlock/timeout")
#Motor operating parameters
self.wheel_d = rospy.get_param("/wheels/diameter")
self.enc_cpr = rospy.get_param("/wheels/encoder/cts_per_rev")
factor = rospy.get_param("/wheels/encoder/factor")
stage_1 = rospy.get_param("/wheels/gearbox/stage1")
stage_2 = rospy.get_param("/wheels/gearbox/stage2")
stage_3 = rospy.get_param("/wheels/gearbox/stage3")
self.accel_const = rospy.get_param("/puffer/accel_const")
self.max_vel_per_s = rospy.get_param("/puffer/max_vel_per_s")
self.tick_per_rev = int(self.enc_cpr * factor * stage_1 * stage_2 * stage_3)
rospy.loginfo(self.tick_per_rev)
rospy.set_param("tick_per_rev", self.tick_per_rev)
self.max_cts_per_s = int((self.max_vel_per_s * self.tick_per_rev)/(math.pi * self.wheel_d))
self.max_accel = int(self.max_cts_per_s * self.accel_const)
rospy.set_param("max_cts_per_s", self.max_cts_per_s)
rospy.set_param("max_accel", self.max_accel)
self.address = rospy.get_param("/motor_controllers/address", 0x80)
self.rc.SetMainVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/main/low", 12.0) * 10),int(rospy.get_param("motor_controllers/battery/main/high", 18.0 ) * 10))
self.rc.SetLogicVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/logic/low") * 10),int(rospy.get_param("motor_controllers/battery/logic/high") * 10))
self.max_current = rospy.get_param("/motor_controllers/current/max_amps")
self.motor_lockout_time = rospy.get_param("/puffer/motor_lockout_time")
m1p = rospy.get_param("/motor_controllers/m1/p")
m1i = rospy.get_param("/motor_controllers/m1/i")
m1d = rospy.get_param("/motor_controllers/m1/d")
m1qpps = rospy.get_param("/motor_controllers/m1/qpps")
m2p = rospy.get_param("/motor_controllers/m2/p")
m2i = rospy.get_param("/motor_controllers/m2/i")
m2d = rospy.get_param("/motor_controllers/m2/d")
m2qpps = rospy.get_param("/motor_controllers/m2/qpps")
self.battery_max_time = rospy.get_param("/battery/max_time")
self.battery_max_volts = rospy.get_param("/battery/max_volts")
self.battery_coef_a = rospy.get_param("/battery/a")
self.battery_coef_b = rospy.get_param("/battery/b")
self.battery_coef_c = rospy.get_param("/battery/c")
self.battery_warning = rospy.get_param("/battery/warning_percent")
self.battery_shutdown = rospy.get_param("/battery/shutdown_percent")
self.rc.SetM1VelocityPID(self.address,m1p, m1i, m1d, m1qpps)
self.rc.SetM2VelocityPID(self.address,m2p, m2i, m2d, m2qpps)
self.rc.WriteNVM(self.address)
time.sleep(0.001)
self.rc.ReadNVM(self.address)
""" JETSON NANO With web interface based on flask with good quality picture
"""
import jetson.inference
import jetson.utils
from threading import Timer, Thread, Lock
from multiprocessing.dummy import Process, Queue
import numpy as np
import time
import cv2
import Jetson.GPIO as GPIO
import sys
import os
import math
GPIO.setwarnings(False) # suppresses GPIO warnings
GPIO.setmode(GPIO.BOARD)
GREEN = 40
GPIO.setup(GREEN, GPIO.OUT)
RED = 38
GPIO.setup(RED, GPIO.OUT)
vis = False # xMode with Display
memmon = False
q_pict = Queue(maxsize=5) # queue for web picts
q_status = Queue(maxsize=5) # queue for web status
# jetson inference networks list
network_lst = [ "ssd-mobilenet-v2", # 0 the best one???
"ssd-inception-v2", # 1
#!/usr/bin/python3
import sys, time
from time import sleep
import Jetson.GPIO as gpio
# Iniciando tudo
channel = 13 # pino de saida que funcionou na Jetson
gpio.setmode(gpio.BOARD) # Usando numeracao raiz da placa
gpio.setwarnings(False) # parar com chatices
gpio.setup(channel, gpio.OUT) # vai funcionar como saida
# Piscando LED rapido V vezes
V = 20
for v in range(1, V):
gpio.output(channel, gpio.HIGH)
sleep(0.3)
gpio.output(channel, gpio.LOW)
sleep(0.3)
gpio.output(channel, gpio.LOW )
# Deixando o led totalmente aceso
gpio.output(channel, gpio.HIGH)
gpio.output(channel, gpio.HIGH)
gpio.output(channel, gpio.HIGH)
def main():
full_scrn = True
parser = argparse.ArgumentParser(description='Darknet Yolo V4 Python Detector')
parser.add_argument("-v", "--video", required=False, default="", help="path to input video file")
parser.add_argument("-s", "--show_video", required=False, type=str2bool, nargs='?', const=True, default=False, help="False for faster")
parser.add_argument("-f", "--save_video", required=False, default="", help="Save Video output as .mp4")
args = parser.parse_args()
global led_matrix, buzzer_f, buzzer_l, buzzer_r, buzzer_s, buzzer_t
global metaMain, netMain, altNames
global fps_time
configPath = "../trained-weights/reference/yolov4-tiny.cfg"
weightPath = "../trained-weights/reference/yolov4-tiny.weights"
metaPath = "../trained-weights/reference/coco.data"
#configPath = "../track/yolov4-tiny.cfg"
#weightPath = "../track/yolov4-tiny_final.weights"
#metaPath = "../track/obj-google.data"
thresh = 0.3
if not os.path.exists(configPath):
raise ValueError("Invalid config path `" +
os.path.abspath(configPath)+"`")
if not os.path.exists(weightPath):
raise ValueError("Invalid weight path `" +
os.path.abspath(weightPath)+"`")
if not os.path.exists(metaPath):
raise ValueError("Invalid data file path `" +
os.path.abspath(metaPath)+"`")
if netMain is None:
netMain = darknet.load_net_custom(configPath.encode(
"ascii"), weightPath.encode("ascii"), 0, 1) # batch size = 1
if metaMain is None:
metaMain = darknet.load_meta(metaPath.encode("ascii"))
if altNames is None:
try:
with open(metaPath) as metaFH:
metaContents = metaFH.read()
import re
match = re.search("names *= *(.*)$", metaContents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as namesFH:
namesList = namesFH.read().strip().split("\n")
altNames = [x.strip() for x in namesList]
except TypeError:
pass
except Exception:
pass
#cap = cv2.VideoCapture(0)
if ( not args.video == "" ):
print("Loading: {}". format(args.video))
cap = cv2.VideoCapture(args.video, cv2.CAP_GSTREAMER)
else:
print("Loading: {}". format(GST_STR))
cap = cv2.VideoCapture(GST_STR, cv2.CAP_GSTREAMER)
#cap = cv2.VideoCapture("v4l2src io-mode=2 device=/dev/video0 ! video/x-raw, format=YUY2, width=1920, height=1080, framerate=60/1 ! nvvidconv ! video/x-raw(memory:NVMM), format=(string)I420 ! nvvidconv ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink sync=false async=false drop=true")
#cap = cv2.VideoCapture("test.mp4")
#cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_OPENGL)
if full_scrn == True:
cv2.namedWindow(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
cv2.moveWindow(WINDOW_NAME, 0, 0)
cv2.resizeWindow(WINDOW_NAME, 640, 360)
else:
cv2.namedWindow(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN)
cv2.resizeWindow(WINDOW_NAME, 640, 360)
#cap.set(3, video_width)
#cap.set(4, video_height)
##This will write the code
if ( not args.save_video == "" ):
fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
if ( args.show_video == True ):
out_video = cv2.VideoWriter( args.save_video , fourcc, 30, (video_width, video_height))
else:
out_video = cv2.VideoWriter( args.save_video , fourcc, 30, (darknet.network_width(netMain), darknet.network_height(netMain)))
#out = cv2.VideoWriter(
# "output.avi", cv2.VideoWriter_fourcc(*"MJPG"), 30.0,
# (darknet.network_width(netMain), darknet.network_height(netMain)))
print("Starting the YOLO loop...")
buzzer_f = VibratorThread( [27], 2, 0.5)
buzzer_l = VibratorThread( [17], 2, 0.3)
buzzer_r = VibratorThread( [18], 2, 0.3)
buzzer_s = VibratorThread( [27], 2, 1)
buzzer_t = VibratorThread( [17,18], 2, 2)
buzzer_f.start()
buzzer_l.start()
buzzer_r.start()
buzzer_s.start()
buzzer_t.start()
# Create an image we reuse for each detect
if ( args.show_video == True ):
darknet_image = darknet.make_image(video_width,video_height,3)
else:
darknet_image = darknet.make_image(darknet.network_width(netMain),
darknet.network_height(netMain),3)
#faulthandler.enable()
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.remove_event_detect(shutdown_pin)
print("Starting demo now! Press CTRL+C to exit")
try:
print("Setting Shutdown to LOW")
GPIO.setup(shutdown_pin, GPIO.OUT)
GPIO.output(shutdown_pin, GPIO.LOW)
##time.sleep(1)
GPIO.cleanup(shutdown_pin)
GPIO.setup(shutdown_pin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) # Set pin 10 to be an input pin and set initial value to be pulled low (off)
GPIO.add_event_detect(shutdown_pin, GPIO.BOTH, callback=button_callback) # Setup event on pin 10 rising edge
finally:
pass
while True:
if cv2.getWindowProperty(WINDOW_NAME, 0) < 0:
# Check to see if the user has closed the window
# If yes, terminate the program
#stop the buzzer
if non_stop_buzzer.isAlive():
try:
non_stop_buzzer.stopit()
non_stop_buzzer.join()
except:
pass
break
prev_time = time.time()
ret, frame_read = cap.read()
if ret != True:
print("Video open failed, run:")
print("sudo systemctl restart nvargus-daemon")
break
cv2.imshow(WINDOW_NAME, frame_read)
frame_rgb = cv2.cvtColor(frame_read, cv2.COLOR_BGR2RGB)
if ( args.show_video == True ):
darknet.copy_image_from_bytes(darknet_image,frame_rgb.tobytes())
else:
frame_resized = cv2.resize(frame_rgb,
(darknet.network_width(netMain),
darknet.network_height(netMain)),
interpolation=cv2.INTER_LINEAR)
darknet.copy_image_from_bytes(darknet_image,frame_resized.tobytes())
#Printing the detections
detections = darknet.detect_image(netMain, metaMain, darknet_image, thresh=thresh, debug=False)
#print(detections)
### Edited the code to perform what what ever you need to
if ( args.show_video == True ):
image = cvDrawBoxes(detections, frame_rgb)
else:
image = cvDrawBoxes(detections, frame_resized)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
myCustomActions(detections,image)
cv2.putText(image,
"FPS: {:.2f}" .format( (1.0 / (time.time()-prev_time)) ),
(10, 25), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 255, 0), 2)
# resize image
#dim = (640, 480)
#resized = cv2.resize(image, dim, interpolation = cv2.INTER_LINEAR)
cv2.imshow(WINDOW_NAME, image)
if ( not args.save_video == "" ):
out_video.write(image)
print("FPS: {:.2f}".format( 1/(time.time()-prev_time) ) )
print("**************")
key = cv2.waitKey(1)
if key == 27 or key == ord("q"): # ESC
try:
buzzer_f.stopit()
buzzer_f.join()
buzzer_l.stopit()
buzzer_l.join()
buzzer_r.stopit()
buzzer_r.join()
buzzer_s.stopit()
buzzer_s.join()
buzzer_t.stopit()
buzzer_t.join()
except:
pass
#stop the buzzer
"""
if non_stop_buzzer.isAlive():
try:
non_stop_buzzer.stopit()
non_stop_buzzer.join()
except:
pass
break
"""
cap.release()
GPIO.cleanup()
if ( not args.save_video == "" ):
out_video.release()
# SPDX-FileCopyrightText: 2021 Melissa LeBlanc-Williams for Adafruit Industries
#
# SPDX-License-Identifier: MIT
"""Tegra T194 pin names"""
import atexit
from Jetson import GPIO
GPIO.setmode(GPIO.TEGRA_SOC)
GPIO.setwarnings(False) # shh!
class Pin:
"""Pins dont exist in CPython so...lets make our own!"""
IN = 0
OUT = 1
LOW = 0
HIGH = 1
PULL_NONE = 0
PULL_UP = 1
PULL_DOWN = 2
id = None
_value = LOW
_mode = IN
def __init__(self, bcm_number):
self.id = bcm_number
def __repr__(self):
return str(self.id)
ROVER = [0, 0, 0]
PDS = [0, 1, 0]
SCIENCE = [0, 1, 1]
TX2 = [1, 0, 0]
PIN_DESC = [ARM, ROVER, PDS, SCIENCE]
SW_PINS = [15, 13, 11]
ser = None
STOP_BYTE = 0x0A
arm_queue = deque()
rover_queue = deque()
science_queue = deque()
gpio.setwarnings(False)
gpio.setmode(gpio.BOARD)
gpio.setup(SW_PINS, gpio.OUT)
gpio.output(SW_PINS, NONE)
def twist_rover_callback(twist_msg):
""" Handles the twist message and sends it to the wheel MCU """
linear, angular = accelerate_twist(twist_msg)
args = twist_to_rover_command(linear, angular)
rover_queue.append(['move_rover', args, ROVER_SELECTED])
def main():
try:
def GPIO_init(channel=40):
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(channel, GPIO.OUT)
# card tap for user login (step1)
import sys
from mfrc522 import SimpleMFRC522
from time import sleep
import Jetson.GPIO as GPIO
reader = SimpleMFRC522()
GPIO.setwarnings(False)
def card_tap():
try:
while True:
id, text = reader.read()
return id
sleep(5)
except KeyboardInterrupt:
GPIO.cleanup()
raise
def __init__(self):
rospy.init_node("wheel_encoders")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
#### parameters #######
self.leftPinA = rospy.get_param('leftPinA',6)
self.leftPinB = rospy.get_param('leftPinB',5)
self.rightPinA = rospy.get_param('rightPinA',13)
self.rightPinB = rospy.get_param('rightPinB',19)
self.ticksPerRevolution = float(rospy.get_param('ticksPerRevolution', 3000))
self.wheelDiameterMm = float(rospy.get_param('wheelDiameterMm', 120))
self.wheelAxisMm = rospy.get_param('wheelAxisMm',210)
self.rate = rospy.get_param('rate', 15)
self.wheel_speeds_topic = rospy.get_param('wheel_speeds_topic', 'wheel_speeds')
self.lwheel_topic = rospy.get_param('lwheel_topic', 'lwheel')
self.rwheel_topic = rospy.get_param('rwheel_topic', 'rwheel')
self.publish_velocities = rospy.get_param('publish_velocities', False)
self.publish_rwheel = rospy.get_param('publish_rwheel', True)
self.publish_lwheel = rospy.get_param('publish_lwheel', True)
self.velocitiy_frame = rospy.get_param('velocitiy_frame', 'odom')
self.velocityUpdateIntervalNs = rospy.get_param('velocityUpdateIntervalNs', 20000000)
self.lticks = 0
self.rticks = 0
self.ldirection = 0
self.rdirection = 0
self.lelapsed_sticks = 0
self.relapsed_sticks = 0
self.then = rospy.Time.now()
# Set the GPIO modes
GPIO.setmode(GPIO.BCM) # BOARD
GPIO.setwarnings(False)
# Set the GPIO Pin mode to be Input
GPIO.setup(self.leftPinA, GPIO.IN)
GPIO.setup(self.leftPinB, GPIO.IN)
GPIO.setup(self.rightPinA, GPIO.IN)
GPIO.setup(self.rightPinB, GPIO.IN)
# Shaft encoders from wheels
self.prev_lpinA = GPIO.input(self.leftPinA)
self.prev_lpinB = GPIO.LOW
self.prev_rpinA = GPIO.input(self.rightPinA)
self.prev_rpinB = GPIO.LOW
self.lValueA = None
self.lValueB = None
self.rValueA = None
self.rValueB = None
# display parameters data
self.display_params()
# Subscriptions
# Publications
self.lwheelPub = rospy.Publisher(self.lwheel_topic, Int32, queue_size=10)
self.rwheelPub = rospy.Publisher(self.rwheel_topic, Int32, queue_size=10)
