class Controller(object):
def __init__(self, goal_vel=0.0):
self.goal_ticks = self.convert_to_ticks(goal_vel)
self.plant = PLANT()
self.wheel_controllers = {
Sensor.KEY_WENC_RIGHT: Wheel_PID_Controller(True, 0.01),
Sensor.KEY_WENC_LEFT: Wheel_PID_Controller(False, 0.01)
}
self.wheel_inputs = [0.0, 0.0]
self.jetbot = Robot()
def input_actions(self, inputs):
for i in range(len(inputs)):
self.jetbot.set_motors(inputs[i][0], inputs[i][1])
time.sleep(inputs[i][2])
self.jetbot.stop()
pass
def handle_callback(self, callback):
ticks_per_second = self.calculate_velocity(callback.data)
if callback.key == Sensor.KEY_WENC_RIGHT:
output = self.right_wheel_controller.calculate_output()
# elif callback.key == Sensor.KEY_WENC_LEFT:
def convert_to_ticks(self, goal_vel):
return (goal_vel / (2 * math.pi) * 8)
class JetbotController:
def __init__(self):
self.robot = Robot()
#self.robot = JetbotDriver().robot
self.left_v = 0.0
self.right_v = 0.0
self.loop = True
self.controll_thread = threading.Thread(target=self._controll_loop)
rospy.init_node('jetbot_cmd_vel_controller')
rospy.Subscriber("/cmd_vel", Twist, self._callback)
def start(self):
self.controll_thread = threading.Thread(target=self._controll_loop)
self.controll_thread.start()
rospy.spin()
def _callback(self, msg):
speed = msg.linear.x
radius = msg.angular.z
self.right_v = (speed + radius) * 0.5
self.left_v = (speed - radius) * 0.5
def _controll_loop(self):
while self.loop:
self.robot.set_motors(self.left_v, self.right_v)
time.sleep(0.1)
def stop(self):
self.loop = False
self.robot.set_motors(0.0, 0.0)
def main():
args = parse_args()
cam = Camera(args)
cam.open()
if not cam.is_opened:
sys.exit('Failed to open camera!')
trt_ssd = TrtSSD(args.model)
cam.start()
# initialize bot
logger.info('initialize robot')
robot = Robot()
logger.info('starting to loop and detect')
loop_and_detect(cam=cam,
trt_ssd=trt_ssd,
conf_th=0.3,
robot=robot,
model=args.model)
logger.info('cleaning up')
robot.stop()
cam.stop()
cam.release()
def __init__(self, goal_vel=0.0):
self.goal_ticks = self.convert_to_ticks(goal_vel)
self.plant = PLANT()
self.wheel_controllers = {
Sensor.KEY_WENC_RIGHT: Wheel_PID_Controller(True, 0.01),
Sensor.KEY_WENC_LEFT: Wheel_PID_Controller(False, 0.01)
}
self.wheel_inputs = [0.0, 0.0]
self.jetbot = Robot()
def __init__(self):
self.robot = Robot()
#self.robot = JetbotDriver().robot
self.left_v = 0.0
self.right_v = 0.0
self.loop = True
self.controll_thread = threading.Thread(target=self._controll_loop)
rospy.init_node('jetbot_cmd_vel_controller')
rospy.Subscriber("/cmd_vel", Twist, self._callback)
def __init__(self):
print("INFO: Starting Formant JetBot Adapter")
# Set global params
self.max_speed = DEFAULT_MAX_SPEED
self.min_speed = DEFAULT_MIN_SPEED
self.speed_deadzone = DEFAULT_SPEED_DEADZONE
self.speed_increment = DEFAULT_SPEED_INCREMENT
self.angular_reduction = DEFAULT_ANGULAR_REDUCTION
self.latitude = DEFAULT_LATITUDE
self.longitude = DEFAULT_LONGITUDE
self.gst_string = DEFAULT_GST_STRING
self.start_speed = DEFAULT_START_SPEED
self.speed = self.start_speed
self.is_shutdown = False
# Store frame rate information to publish
self.camera_width = 0
self.camera_height = 0
self.camera_frame_timestamps = collections.deque([], maxlen=100)
self.camera_frame_sizes = collections.deque([], maxlen=100)
# Create clients
self.robot = Robot()
self.ina219 = INA219(addr=0x41)
self.fclient = FormantClient(ignore_throttled=True,
ignore_unavailable=True)
self.update_from_app_config()
self.publish_online_event()
self.fclient.register_command_request_callback(
self.handle_command_request)
self.fclient.register_teleop_callback(self.handle_teleop,
["Joystick", "Buttons"])
print("INFO: Starting speed thread")
threading.Thread(target=self.publish_speed, daemon=True).start()
print("INFO: Starting motor states thread")
threading.Thread(target=self.publish_motor_states, daemon=True).start()
print("INFO: Starting location thread")
threading.Thread(target=self.publish_location, daemon=True).start()
print("INFO: Starting battery state thread")
threading.Thread(target=self.publish_battery_state,
daemon=True).start()
print("INFO: Starting camera stats thread")
threading.Thread(target=self.publish_camera_stats, daemon=True).start()
# Start the camera feed
self.publish_camera_feed()
def __init__(self):
super(RobotEnv, self).__init__()
#n_actions = 3
n_actions = 2
self.action_space = spaces.Discrete(n_actions)
self.observation_space = spaces.Box(0, 255, [224, 224, 3], dtype=np.uint8)
self.state = None
self.reward = 0
self.speed = 20
self.speed_threshold = 10
self.image = None
self.prev_image = None
self.robot = Robot()
def __init__(self, port=8765):
super().__init__('ws_server')
self.cmd_sub_ = self.create_subscription(Twist, 'cmd_vel',
self.cmd_callback, 10)
self.odom_sub_ = self.create_subscription(Odometry, 'rs_t265/odom',
self.odom_callback, 10)
self.vel_cmd_ = 0.0
self.yaw_rate_cmd_ = 0.0
self.yaw_rate_error_int_ = 0.0
self.vel_error_int_ = 0.0
if not IS_LOCAL:
self.robot = Robot()
def __init__(self):
print('Setting up camera')
self.camera = Camera.instance(width=224, height=224)
print('Set up camera')
self.robot = Robot()
self.completed = False
# Collision Avoidance.
print('Loading CA model')
self.ca_model = torchvision.models.alexnet(pretrained=False)
self.ca_model.classifier[6] = torch.nn.Linear(
self.ca_model.classifier[6].in_features, 2)
#self.ca_model.load_state_dict(torch.load('best_model_nvidia.pth'))
self.device = torch.device('cuda')
self.ca_model = self.ca_model.to(self.device)
print('Loaded CA model')
self.mean = 255.0 * torch.Tensor(np.array([0.485, 0.456, 0.406
])).cuda().half()
self.std = 255.0 * torch.Tensor(np.array([0.485, 0.456, 0.406
])).cuda().half()
self.normalize = torchvision.transforms.Normalize(self.mean, self.std)
# Road following support variables.
self.angle = 0.0
self.angle_last = 0.0
# Instantiating the road following network.
print('Loading RF model')
self.rf_model = torchvision.models.resnet18(pretrained=False)
self.rf_model.fc = torch.nn.Linear(512, 2)
self.rf_model.load_state_dict(torch.load('best_steering_model_xy.pth'))
self.rf_model = self.rf_model.to(self.device)
self.rf_model = self.rf_model.eval().half()
print('Loaded RF Model')
# Initializing additional variables.
self.speed_gain = 0.12
self.steering_gain = 0
self.steering_dgain = 0.1
self.steering_bias = 0.0
self.starttime = 0
self.cumulative_angle = -1
self.pitstop = []
self.startposition = []
self.start_num = -1
self.baton_callback = None
self.pathpoints_callback = None
self.pathpoints = [[]]
# Add proper value below.
self.proportionality_const = -1
self.image_widget = ipywidgets.Image()
self.previous_position = -1
traitlets.dlink((self.camera, 'value'), (self.image_widget, 'value'),
transform=bgr8_to_jpeg)
def listener():
rospy.init_node('twist_to_motor')
rospy.Subscriber('dtw_robot/diff_drive_controller/cmd_vel', Twist,
twist_cb)
rate = rospy.Rate(10)
robot = Robot()
global init_flag
init_flag = 1
while not rospy.is_shutdown():
robot.set_motors(v_l, v_r)
print(v_l)
rate.sleep()
class PLANT(object):
def __init__(self):
self.jetbot = Robot(
) # Robot() is jetbot API for inputting motor controls, no need to manage PWM inputs ourselves.
# Moves jetbot by controlling individual wheels.
# inputs: 2D tuple with list of motor inputs and time for each motor input
# inputs[i][0]: Left motor input for ith command
# inputs[i][1]: Right motor input for ith command
# inputs[i][2]: DeltaT for ith command
def move(self, inputs):
for i in range(len(inputs)):
self.jetbot.set_motors(inputs[i][0], inputs[i][1])
time.sleep(inputs[i][2])
self.jetbot.stop()
pass
def start(self):
self.device = torch.device('cuda')
print('Loading model...')
# create model
self.model = torchvision.models.alexnet(pretrained=False)
self.model.classifier[6] = torch.nn.Linear(
self.model.classifier[6].in_features, 2)
self.model.load_state_dict(torch.load(self.collision_model))
self.model = self.model.to(self.device)
# create robot
self.robot = Robot()
print('Initializing camera...')
# create camera
self.camera = Camera.instance(width=224, height=224)
print('Running...')
self.camera.observe(self._update, names='value')
def kill(sig, frame):
print('Shutting down...')
self.camera.stop()
signal.signal(signal.SIGINT, kill)
self.camera.thread.join()
def __init__(self):
self.camera = Camera.instance(width=224, height=224)
self.robot = Robot()
#Collision Avoidance
self.ca_model = torchvision.models.alexnet(pretrained=False)
self.ca_model.classifier[6] = torch.nn.Linear(
self.ca_model.classifier[6].in_features, 2)
self.ca_model.load_state_dict(torch.load('best_model.pth'))
self.device = torch.device('cuda')
self.ca_model = self.ca_model.to(self.device)
self.mean = torch.Tensor([0.485, 0.456, 0.406]).cuda().half()
self.std = torch.Tensor([0.229, 0.224, 0.225]).cuda().half()
self.normalize = torchvision.transforms.Normalize(self.mean, self.std)
#Road following support variables
self.angle = 0.0
self.angle_last = 0.0
# Instantiating the road following network.
self.rf_model = torchvision.models.resnet18(pretrained=False)
self.rf_model.fc = torch.nn.Linear(512, 2)
self.rf_model.load_state_dict(torch.load('best_steering_model_xy.pth'))
self.rf_model = self.rf_model.to(self.device)
self.rf_model = self.rf_model.eval().half()
self.speed_gain = 0.12
self.steering_gain = 0
self.steering_dgain = 0.1
self.steering_bias = 0.0
self.t_unit_dist = 0.04
self.starttime = 0
self.cumulative_angle = -1
self.pitstop = []
self.startposition = []
self.pathpoints = [[]]
self.proportionality_const = -1 # TODO : Add the proper value
async def jsEvent(dev):
xAxis = 0
yAxis = 0
speed = 0
torque = 0
midAxis = 128
leftMotorSpeed = 0
rightMotorSpeed = 0
robot = Robot()
async for ev in dev.async_read_loop():
if (ev.type == 3): # analog
# get the Axis values from the js
if (ev.code == 1): # Y axis
yAxis = ev.value # maintain state
elif (ev.code == 0): # X axis
xAxis = ev.value # maintain state for both
# Y axis values represent the speed, X the rotation
speed = round(scale_js_Y(yAxis),2)
torque = round(scale_js_X(xAxis),2)
if (speed == 0 and torque == 0):
robot.stop()
else:
# combine speed and torque
leftMotorSpeed = speed
rightMotorSpeed = speed
if (xAxis > midAxis): # rotate right
leftMotorSpeed = round((leftMotorSpeed + torque), 2)
rightMotorSpeed = round((rightMotorSpeed - torque), 2)
else: # rotate left
leftMotorSpeed = round((leftMotorSpeed - torque), 2)
rightMotorSpeed = round((rightMotorSpeed + torque), 2)
if (leftMotorSpeed > 1): leftMotorSpeed = 1
elif (leftMotorSpeed < -1): leftMotorSpeed = -1
elif (rightMotorSpeed > 1): rightMotorSpeed = 1
elif (rightMotorSpeed < -1): rightMotorSpeed = -1
# forward
#robot.forward(motorSpeed)
#print(repr(ev))
#print(leftMotorSpeed, rightMotorSpeed)
print('x-axis', xAxis, 'y-axis', yAxis, 'speed: ',speed, 'torque: ', torque, 'left: ', leftMotorSpeed, 'right: ', rightMotorSpeed)
def main():
print("import finished.")
sleep(2)
robot = Robot()
r_speed = 0.25
l_speed = 0.25
distance = 50
t1 = threading.Thread(target=drive, \
args=(s1, robot, r_speed, l_speed, distance))
t1.start()
def main(args=None):
rclpy.init(args=args)
robot = Robot()
motorsNode = MotorsNode(robot)
rclpy.spin(motorsNode)
sb.destroy_node()
rclpy.shutdown()
def collision_avoidance():
global normalize, device, model, mean, camera, robot
i_frame = -1
model = torchvision.models.alexnet(pretrained=False)
model.classifier[6] = torch.nn.Linear(model.classifier[6].in_features, 2)
model.load_state_dict(torch.load('best_model.pth'))
device = torch.device('cuda')
model = model.to(device)
mean = 255.0 * np.array([0.485, 0.456, 0.406])
stdev = 255.0 * np.array([0.229, 0.224, 0.225])
camera = Camera.instance(width=224, height=224)
normalize = torchvision.transforms.Normalize(mean, stdev)
robot = Robot()
robot.stop()
now = time.time()
stop_time = now + 120
while time.time() < stop_time:
i_frame += 1
if i_frame % 2 == 0:
update({'new':
camera.value}) # we call the function once to intialize
#cv2.imshow(camera.value)
#camera.observe(update, names='value')
robot.stop()
camera.stop()
def main():
robot = Robot()
pb_file = 'following/faster_rcnn_resnet50_coco_2018_01_28/frozen_inference_graph.pb'
cfg_file = 'following/faster_rcnn_resnet50_coco_2018_01_28.pbtxt'
cvNet = cv.dnn.readNetFromTensorflow(pb_file, cfg_file)
cap = cv.VideoCapture(camSet)
try:
while cap.isOpened():
ret, frame = cap.read()
# sendUDP(frame)
img = frame
rows = img.shape[0]
cols = img.shape[1]
cvNet.setInput(
cv.dnn.blobFromImage(img,
size=(300, 300),
swapRB=True,
crop=False))
cvOut = cvNet.forward()
for detection in cvOut[0, 0, :, :]:
score = float(detection[2])
if score > 0.3:
print(detection)
# Stop the program on the ESC key
if cv.waitKey(30) == 27:
break
except Exception as e:
print(e.args[0])
finally:
robot.stop()
cap.release()
sock.close()
class RobotController():
SPEED = 0.4
MAX_MOTORLIMIT = 0.3
MIN_MOTORLIMIT = 0.0
def __init__(self):
self.robot = Robot()
def action(self, steering, throttle):
steering = float(steering)
throttle = float(throttle)
self.robot.left_motor.value = max(
min(throttle + steering, self.MAX_MOTORLIMIT), self.MIN_MOTORLIMIT)
self.robot.right_motor.value = max(
min(throttle - steering, self.MAX_MOTORLIMIT), self.MIN_MOTORLIMIT)
def stop(self):
self.robot.stop()
def __init__(self):
# jetbot setting
self.robot = Robot()
self.avoidance_status = False
self.cruise_status = False
self.move_arrow = 'stop'
self.n = 0.0
self.direction = ""
self.pw = 1
self.pw_c = 1.5
self.left_power = (0.15)
self.right_power = (0.145)
# deep learning model setting
self.set_detect_model()
self.set_seg_model()
self.roi = [(0, 120),(80, 60),(160, 120),]
# camera setting
self.cap = cv2.VideoCapture(1)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) # width
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) # height
self.cap.set(cv2.CAP_PROP_FPS, 10)
(self.grabbed, self.frame) = self.cap.read()
# thread
threading.Thread(target=self.update, args=()).start()
class Wheels(object):
def __init__(self):
self.robot = Robot()
self.stop()
def cmd_vel_process(self, linear_speed, angular_speed):
if angular_speed != 0:
# We turn
if angular_speed > 0:
self.go_left(abs(angular_speed))
else:
self.go_right(abs(angular_speed))
else:
if linear_speed > 0:
self.go_forward(abs(linear_speed))
elif linear_speed < 0:
self.go_backward(abs(linear_speed))
else:
# We have to stop
self.stop()
def stop(self):
self.robot.stop()
def go_forward(self, speed=0.4):
self.robot.forward(speed)
def go_backward(self, speed=0.4):
self.robot.backward(speed)
def go_left(self, speed=0.3):
self.robot.left(speed)
def go_right(self, speed=0.3):
self.robot.right(speed)
def main():
# parser = argparse.ArgumentParser(description='PD Control Tester')
# parser.add_argument("-p", type=str, help='Port of the serial', required=True)
# parser.add_argument("-b", type=int, help="Baudrate of the serial", default=9600)
# parser.add_argument("-x", type=float, help="x position of Goal in cm", default=100)
# parser.add_argument("-y", type=float, help="y position of Goal in cm", default=0)
# parser.add_argument("-theta", type=float, help="theta of Goal", default=0)
# args = parser.parse_args()
print("import finished.")
sleep(2)
robot = Robot()
l_speed = 1.0
r_speed = 1.0
distance = 100
t1 = threading.Thread(target=drive, \
args=(s1, robot, l_speed, r_speed, distance))
t1.start()
def main():
parser = argparse.ArgumentParser(description='Khan')
parser.add_argument("-p", type=str, help='Port of the serial', default='/dev/ttyACM0')
parser.add_argument("-b", type=int, help="Baudrate of the serial", default=9600)
parser.add_argument("-speed", type=float, help="speed in cm/s", default=10)
args = parser.parse_args()
# khan = Khan(args.p, args.b)
robot = Robot()
imageHandler = ImageHandler(width=640, height=480)
laneDetector = LaneDetector(imageHandler.camera)
print("ready to run, wait for 1 second...")
sleep(1)
robot.set_motors(0.2,0.3)
t1 = threading.Thread(target=markROI(), args=(laneDetector,))
t1.start()
sleep(10)
robot.stop()
#For Robot
from jetbot import Robot
robot = Robot()
robotSpeed = 0.4
robot.stop()
import jetson.inference
import jetson.utils
net = jetson.inference.detectNet("ssd-mobilenet-v2", threshold=0.5)
#camera = jetson.utils.videoSource("csi://0") # '/dev/video0' for V4L2
camera = jetson.utils.videoSource("/dev/video1") # '/dev/video0' for V4L2
display = jetson.utils.videoOutput("display://0") # 'my_video.mp4' for file
#display = jetson.utils.videoOutput("rtp://192.168.1.169:1234") # 'my_video.mp4' for file
index = 0
width = 0
location = 0
while display.IsStreaming():
img = camera.Capture()
detections = net.Detect(img)
display.Render(img)
display.SetStatus("Object Detection | Network {:.0f} FPS".format(
net.GetNetworkFPS()))
for detection in detections:
index = detections[0].ClassID
width = (detections[0].Width)
location = (detections[0].Center[0])
max=1.0,
orientation='vertical')
camera_link = traitlets.dlink((camera, 'value'), (image, 'value'),
transform=bgr8_to_jpeg)
display(widgets.HBox([image, blocked_slider, gpu_slider]))
#16 seconds
# We'll also create our robot instance which we'll need to drive the motors.
# In[6]:
from jetbot import Robot
robot = Robot()
robot.stop()
# Next, we'll create a function that will get called whenever the camera's value changes. This function will do the following steps
#
# 1. Pre-process the camera image
# 2. Execute the neural network
# 3. While the neural network output indicates we're blocked, we'll turn left, otherwise we go forward.
# In[7]:
import torch.nn.functional as F
import time
#DMF Added gpu_usage() section
import os
import cv2
import subprocess
from utils.visualization import BBoxVisualization
from utils.yolo_with_plugins import TrtYOLO
import pycuda.autoinit
import time, queue, threading
from utils.yolo_classes import get_cls_dict
import numpy as np
from datetime import datetime
from jetbot import Robot
robot = Robot()
image_root = os.path.join("Pictures",
datetime.utcnow().strftime('%Y%m%d%H%M%S'))
os.makedirs(image_root)
import RPi.GPIO as GPIO
#import time
GPIO.setmode(GPIO.BOARD
) # Set Jetson Nano to use pin number when referencing GPIO pins.
# Pin Definitions
trig_output_pin = 13 #发射PIN，J41_BOARD_PIN13---gpio14/GPIO.B06/SPI2_SCK
echo_input_pin = 15 #接收PIN，J41_BOARD_PIN18---gpio15/GPIO.B07/SPI2_CS0
LED_RED = 11
LED_YELLOW = 31
from jetcam.csi_camera import CSICamera
import cv2
import time
from jetbot import Robot
import logging
# from threading import Thread
import threading
# from jetcam.usb_camera import USBCamera
robot = Robot()
camera = CSICamera(width=224, height=224)
# camera = USBCamera(width=224, height=224)
camera.running = True
# def thread_function(name):
# print("capturing")
# cv2.imwrite("dataset/{0}-{1}.jpg".format("A", time.time()), camera.value)
# print("done")
# logging.info("Thread %s: starting", name)
# time.sleep(.2)
# x = threading.Thread(target=thread_function, args=(100,))
# x.start()
#
i = 0
while True:
print("capturing")
cv2.imwrite("dataset/{0}-{1}.jpg".format("A", time.time()), camera.value)
def run(remotecontrol=True, cameracapture=False):
global dualshock
global robot
global axis
global continuouscap
global continuouscaptime
global neuralnet_latched
global nnproc
prevShutter = False
meaningful_input_time = None
neuralnet_input_time = None
cam_cap_time = None
last_speed_debug_time = datetime.datetime.now()
last_tick_debug_time = datetime.datetime.now()
print("Remote Control script running! ", end=" ")
if remotecontrol:
print("in RC mode")
elif cameracapture:
print("in CAMERA mode")
else:
print("unknown mode, quitting")
quit()
if remotecontrol:
try:
robot = Robot()
except Exception as ex:
sys.stderr.write("Failed to initialize motor drivers, error: %s" %
(str(ex)))
robot = None
while True:
dualshock = get_dualshock4()
if dualshock != None:
print("DualShock4 found, %s" % str(dualshock))
else:
time.sleep(2)
now = datetime.datetime.now()
delta_time = now - last_tick_debug_time
if delta_time.total_seconds() > 5:
last_tick_debug_time = now
sys.stderr.write("tick %s" % (str(now)))
while dualshock != None:
now = datetime.datetime.now()
delta_time = now - last_tick_debug_time
if delta_time.total_seconds() > 5:
last_tick_debug_time = now
sys.stderr.write("tick %s" % (str(now)))
try:
event = dualshock.read_one()
if event != None:
gamepad_event_handler(event,
is_remotecontrol=remotecontrol,
is_cameracapture=cameracapture)
else:
time.sleep(0)
all_btns = dualshock.active_keys()
if remotecontrol:
meaningful_input = False # meaningful input means any buttons pressed or the stick has been moved
if TRIANGLE in all_btns:
neuralnet_latched = False
mag_dpad, ang_dpad = axis_vector(
axis[DPAD_X], axis[DPAD_Y])
mag_left, ang_left = axis_vector(
axis_normalize(axis[LEFT_X], curve=0),
axis_normalize(axis[LEFT_Y], curve=0))
mag_right, ang_right = axis_vector(
axis_normalize(axis[RIGHT_X], curve=0),
axis_normalize(axis[RIGHT_Y], curve=0))
now = datetime.datetime.now()
if mag_dpad != 0 or mag_left > 0.1 or mag_right > 0.1:
meaningful_input = True
if meaningful_input_time == None:
print("meaningful input!")
meaningful_input_time = now
elif meaningful_input_time != None: # user may have let go, check for timeout
delta_time = now - meaningful_input_time
if delta_time.total_seconds() > 2:
print(
"No meaningful input, stopping robot motors"
)
meaningful_input = False
meaningful_input_time = None
if robot != None:
robot.stop()
else:
meaningful_input = True
use_nn = False
if SQUARE in all_btns:
neuralnet_latched = True
if TRIANGLE in all_btns:
neuralnet_latched = False
if neuralnet_latched or CROSS in all_btns:
use_nn = True
if meaningful_input == False and nnproc is not None: # remote control inputs always override neural net inputs
while sys.stdin in select.select([sys.stdin], [],
[], 0)[0]:
line = sys.stdin.readline()
if line:
try:
axis[NN_THROTTLE] = int(line[0:3])
axis[NN_STEERING] = int(line[3:])
neuralnet_input_time = now
except:
pass
if neuralnet_input_time != None and use_nn:
delta_time = now - neuralnet_input_time
if delta_time.total_seconds() < 5:
meaningful_input = True
meaningful_input_time = now
if meaningful_input:
left_speed = 0
right_speed = 0
ignore_dpad = False
#ignore_rightstick = True
if use_nn:
start_nn_proc(
) # this will check if process has already started
left_speed, right_speed = axis_mix(
axis_normalize(axis[NN_STEERING]),
axis_normalize(255 - axis[NN_THROTTLE]))
elif mag_dpad != 0 and ignore_dpad == False:
left_speed, right_speed = axis_mix(
float(axis[DPAD_X]) / 3.0, axis[DPAD_Y])
#elif mag_left > mag_right or ignore_rightstick == True:
# left_speed, right_speed = axis_mix(axis_normalize(axis[LEFT_X]), axis_normalize(axis[LEFT_Y]))
# if ignore_rightstick == False:
# left_speed /= 2
# right_speed /= 2
else:
# left_speed, right_speed = axis_mix(axis_normalize(axis[RIGHT_X]), axis_normalize(axis[RIGHT_Y]))
left_speed, right_speed = axis_mix(
axis_normalize(axis[RIGHT_X]),
axis_normalize(axis[LEFT_Y]))
if robot != None:
robot.set_motors(left_speed, right_speed)
delta_time = now - last_speed_debug_time
if delta_time.total_seconds() >= 1:
if use_nn:
print("nn -> ", end="")
print("leftmotor: %.2f rightmotor: %.2f" %
(left_speed, right_speed))
last_speed_debug_time = now
elif cameracapture:
now = datetime.datetime.now()
need_cap = False
if L1 in all_btns:
if prevShutter == False:
if continuouscaptime != None:
timedelta = now - continuouscaptime
if timedelta.total_seconds() > 0.5:
continuouscap = not continuouscap
else:
continuouscap = not continuouscap
prevShutter = True
else:
prevShutter = False
if continuouscap:
cam_cap_time = now
need_cap = L1
else:
if cam_cap_time != None:
timedelta = now - cam_cap_time
if timedelta.total_seconds() < 1:
#need_cap = OTHERCODE
pass
else:
cam_cap_time = None
if need_cap != False:
snapname = get_snap_name(need_cap)
print("saving running pic: " + snapname)
cam_capture(snapname)
cam_frame_time = now
while True:
now = datetime.datetime.now()
cam_frame_timedelta = now - cam_frame_time
if cam_frame_timedelta.total_seconds() >= 0.01:
break
event = dualshock.read_one()
if event != None:
gamepad_event_handler(
event,
is_remotecontrol=False,
is_cameracapture=True)
except OSError:
print("DualShock4 disconnected")
dualshock = None
if remotecontrol:
end_cam_proc()
if robot != None:
robot.stop()
# input_image.value = bgr8_to_jpeg(cannyed_image)
center_image_point = [height - 1, width / 2 - 1]
midlane = find_midlane(center_ptrs, center_image_point)
seta = find_degree(center_image_point, midlane)
cv2.line(img, (int(center_image_point[1]), int(center_image_point[0])),
(int(midlane[1]), int(midlane[0])), (0, 0, 255), 3)
cv2.putText(img, f'{seta}', (int(midlane[1]), int(midlane[0]) - 5),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255, 0, 0), 1)
return img, img_op, seta
# In[3]:
robot = Robot()
left_slider = widgets.FloatSlider(description='left',
min=-1.0,
max=1.0,
step=0.01,
orientation='vertical')
right_slider = widgets.FloatSlider(description='right',
min=-1.0,
max=1.0,
step=0.01,
orientation='vertical')
left_link = traitlets.link((left_slider, 'value'), (robot.left_motor, 'value'))
right_link = traitlets.link((right_slider, 'value'),
(robot.right_motor, 'value'))
# In[31]:
def __init__(self):
robot = Robot()
self.mobile = MobileController(10, robot)
#rospy.Subscriber('/joy', Joy, self.joy_stick_callback)
rospy.Subscriber("/jetbot/cmd_vel", Twist, self.cmd_vel_callback)
