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)