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)
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(): 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 move(direction): robot = Robot() if direction == "forward": robot.forward(0.4) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "left": robot.left(0.3) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "right": robot.right(0.3) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "backward": robot.backward(0.4) time.sleep(0.5) robot.stop() return render_template('index.html') else: return render_template('index.html') return render_template('index.html')
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
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(): 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()
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()
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)
#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])
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()
class FormantJetBotAdapter: 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 __del__(self): self.is_shutdown = True def publish_speed(self): while not self.is_shutdown: # self.fclient.post_numeric("speed", self.speed) self.fclient.post_numericset( "Speed", {"speed": (self.speed + self.speed_deadzone, "m/s")}, ) time.sleep(1.0) def publish_motor_states(self): while not self.is_shutdown: self.fclient.post_numeric("Motor Speed", self.robot.left_motor.value, {"value": "left"}) self.fclient.post_numeric("Motor Speed", self.robot.right_motor.value, {"value": "right"}) time.sleep(0.5) def publish_location(self): while not self.is_shutdown: self.fclient.post_geolocation("Location", self.latitude, self.longitude) time.sleep(10.0) def publish_battery_state(self): while not self.is_shutdown: bus_voltage = self.ina219.getBusVoltage_V() shunt_voltage = self.ina219.getShuntVoltage_mV() / 1000 current = self.ina219.getCurrent_mA() / 1000 charging = False if shunt_voltage > 0.01 and current > 0.01: charging = True # approximate battery charge percentage calibration now = bus_voltage - MIN_DISCHARGING_VOLTAGE full = MAX_DISCHARGING_VOLTAGE - MIN_DISCHARGING_VOLTAGE charge_percentage = now / full * 100 if charging: now = bus_voltage - MIN_CHARGING_VOLTAGE full = MAX_CHARGING_VOLTAGE - MIN_CHARGING_VOLTAGE charge_percentage = now / full * 100 if charge_percentage >= 100: charge_percentage = 100 self.fclient.post_battery("Battery State", charge_percentage, voltage=bus_voltage, current=current) self.fclient.post_bitset("Battery Charging", { "charging": charging, "discharging": not charging }) time.sleep(1.0) def publish_camera_stats(self): while not self.is_shutdown: try: timestamps = list(self.camera_frame_timestamps) sizes = list(self.camera_frame_sizes) except: print("ERROR: deque mutated while iterating") pass length = len(timestamps) if length > 2: size_mean = mean(sizes) size_stdev = stdev(sizes) jitter = self.calculate_jitter(timestamps) oldest = timestamps[0] newest = timestamps[-1] diff = newest - oldest if diff > 0: hz = length / diff self.fclient.post_numericset( "Camera Statistics", { "Rate": (hz, "Hz"), "Mean Size": (size_mean, "bytes"), "Std Dev": (size_stdev, "bytes"), "Mean Jitter": (jitter, "ms"), "Width": (self.camera_width, "pixels"), "Height": (self.camera_height, "pixels"), }, ) time.sleep(5.0) def publish_camera_feed(self): cap = cv2.VideoCapture(self.gst_string, cv2.CAP_GSTREAMER) if cap is None: print("ERROR: Could not read from video capture source.") sys.exit() while not self.is_shutdown: _, image = cap.read() try: encoded = cv2.imencode(".jpg", image)[1].tostring() self.fclient.post_image("Camera", encoded) # Track stats for publishing self.camera_frame_timestamps.append(time.time()) self.camera_frame_sizes.append(len(encoded) * 3 / 4) self.camera_width = image.shape[1] self.camera_height = image.shape[0] except: print("ERROR: encoding failed") def publish_online_event(self): try: commit_hash_file = ( "/home/jetbot/formant-jetbot-adapter/.git/refs/heads/main") with open(commit_hash_file) as f: commit_hash = f.read() except Exception: print( "ERROR: formant-jetbot-adapter repo must be installed at " "/home/jetbot/formant-jetbot-adapter to receive online event") self.fclient.create_event( "Formant JetBot adapter online", notify=False, tags={"hash": commit_hash.strip()}, ) def update_from_app_config(self): print("INFO: Updating configuration ...") self.max_speed = float( self.fclient.get_app_config("max_speed", DEFAULT_MAX_SPEED)) self.min_speed = float( self.fclient.get_app_config("min_speed", DEFAULT_MIN_SPEED)) self.speed_deadzone = float( self.fclient.get_app_config("speed_deadzone", DEFAULT_SPEED_DEADZONE)) self.speed_increment = float( self.fclient.get_app_config("speed_increment", DEFAULT_SPEED_INCREMENT)) self.angular_reduction = float( self.fclient.get_app_config("angular_reduction", DEFAULT_ANGULAR_REDUCTION)) self.latitude = float( self.fclient.get_app_config("latitude", DEFAULT_LATITUDE)) self.longitude = float( self.fclient.get_app_config("longitude", DEFAULT_LONGITUDE)) self.gst_string = self.fclient.get_app_config("gst_string", DEFAULT_GST_STRING) self.start_speed = float( self.fclient.get_app_config("start_speed", DEFAULT_START_SPEED)) def handle_command_request(self, request): if request.command == "jetbot.nudge_forward": self._handle_nudge_forward() self.fclient.send_command_response(request.id, success=True) elif request.command == "jetbot.nudge_backward": self._handle_nudge_backward() self.fclient.send_command_response(request.id, success=True) elif request.command == "jetbot.update_config": self.update_from_app_config() self.fclient.send_command_response(request.id, success=True) else: self.fclient.send_command_response(request.id, success=False) def handle_teleop(self, control_datapoint): if control_datapoint.stream == "Joystick": self.handle_joystick(control_datapoint) elif control_datapoint.stream == "Buttons": self.handle_buttons(control_datapoint) def handle_joystick(self, joystick): left_motor_value = 0.0 right_motor_value = 0.0 # Add contributions from the joysticks # TODO: turn this into a circle, not a square left_motor_value += (self.speed * joystick.twist.angular.z * self.angular_reduction) right_motor_value += (-self.speed * joystick.twist.angular.z * self.angular_reduction) left_motor_value += self.speed * joystick.twist.linear.x right_motor_value += self.speed * joystick.twist.linear.x # Improve the deadzone if left_motor_value > 0: left_motor_value += self.speed_deadzone elif left_motor_value < 0: left_motor_value -= self.speed_deadzone if right_motor_value > 0: right_motor_value += self.speed_deadzone elif right_motor_value < 0: right_motor_value -= self.speed_deadzone # Set the motor values self.robot.left_motor.value = left_motor_value self.robot.right_motor.value = right_motor_value def handle_buttons(self, _): if _.bitset.bits[0].key == "nudge forward": self._handle_nudge_forward() elif _.bitset.bits[0].key == "nudge backward": self._handle_nudge_backward() elif _.bitset.bits[0].key == "speed +": self._handle_increase_speed() elif _.bitset.bits[0].key == "speed -": self._handle_decrease_speed() def _handle_nudge_forward(self): self.fclient.post_text("Commands", "nudge forward") self.robot.forward(self.speed + self.speed_deadzone) time.sleep(0.5) self.robot.stop() def _handle_nudge_backward(self): self.fclient.post_text("Commands", "nudge backward") self.robot.backward(self.speed + self.speed_deadzone) time.sleep(0.5) self.robot.stop() def _handle_increase_speed(self): self.fclient.post_text("Commands", "increase speed") if self.speed + self.speed_increment <= self.max_speed: self.speed += self.speed_increment else: self.speed = self.max_speed def _handle_decrease_speed(self): self.fclient.post_text("Commands", "decrease speed") if self.speed - self.speed_increment >= self.min_speed: self.speed -= self.speed_increment else: self.speed = self.min_speed def calculate_jitter(self, timestamps): length = len(self.camera_frame_timestamps) oldest = self.camera_frame_timestamps[0] newest = self.camera_frame_timestamps[-1] step_value = (newest - oldest) / length # Make a list of the difference between the expected and actual step sizes jitters = [] for n in range(length - 1): if n > 0: jitter = abs((timestamps[n] - timestamps[n - 1]) - step_value) jitters.append(jitter) return mean(jitters)
class Navigation: 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 preprocess_detect_collision(self, camera_value): """Preprocessing for collision avoidance.""" x = camera_value x = cv2.cvtColor(x, cv2.COLOR_BGR2RGB) x = x.transpose((2, 0, 1)) x = torch.from_numpy(x).float() x = self.normalize(x) x = x.to(self.device) x = x[None, ...] print("Collision Detection Preprocessing done.") return x def detect_collision(self, change): """This will determine the next start point which will be which will be demarcated by the presence of another bot.""" # Collision avoidance has to be trained to detect a bot as # an obstacle. This will then be called in the road following function. x = change['new'] x = self.preprocess_detect_collision(x) y = self.ca_model(x) y = F.softmax(y, dim=1) prob_blocked = float(y.flatten()[0]) # Debugging : print('Prob: ', prob_blocked) if prob_blocked < 0.5: print('Free') return False else: print('Blocked') return True def update_cumulative_angle(self, angle): """Update the cumulative angle.""" self.cumulative_angle = angle * self.proportionality_const + self.cumulative_angle # self.angle is proportional to actual angle def log_data(self, angle): """Save the path coordinates.""" self.update_cumulative_angle(angle) if (time.time() - self.starttime > 0.2): # every 0.2 seconds log the data print('Logging Data') # with the assumption that every 0.2s it moves 2 cm displacement_vec = [ 2 * math.cos(self.cumulative_angle), 2 * math.sin(self.cumulative_angle) ] new_position = [ self.previous_position[0] + displacement_vec[0], self.previous_position[1] + displacement_vec[1] ] self.pathpoints.append(new_position) self.previous_position = new_position self.starttime = time.time() def update_motor_values(self, pid): """Update the left and right motor values.""" steering_value = pid + self.steering_bias self.robot.left_motor.value = max( min(self.speed_gain + steering_value, 1.0), 0.0) self.robot.right_motor.value = max( min(self.speed_gain - steering_value, 1.0), 0.0) def preprocess_follow_road(self, image): """Preprocesses the image for road following.""" image = PIL.Image.fromarray(image) image = transforms.functional.to_tensor(image).to(self.device).half() image.sub_(self.mean[:, None, None]).div_(self.std[:, None, None]) return image[None, ...] def follow_road(self, change): """Function for road following""" self.starttime = time.time() if self.cumulative_angle == -1: self.cumulative_angle = self.startpoint[2] self.previous_position = self.startpoint[0:2] collision_detected = self.detect_collision(change) if self.completed: self.robot.stop() elif collision_detected: self.completed = True self.next_bot_detected() image = change['new'] print('Processing camera frame for road processing') xy = self.rf_model(self.preprocess_follow_road( image)).detach().float().cpu().numpy().flatten() x = xy[0] y = (0.5 - xy[1]) / 2.0 self.angle = np.arctan2(x, y) # Debugging : print('Angle = ', self.angle) pid = self.angle * self.steering_gain + ( self.angle - self.angle_last) * self.steering_dgain print('Update Motor Values') self.update_motor_values(pid) print('Logging Data') self.log_data(self.angle) def next_bot_detected(self): """Call the baton and the pathpoints callback upon detecting a bot.""" self.camera.unobserve(self.follow_road, names='value') time.sleep(0.1) self.robot.stop() self.baton_callback(self.start_num) self.pathpoints_callback(self.pathpoints) def move_forward(self, distance, turbo=0): """Move forward for a certain distance.""" if (turbo == 1): self.linear_velocity_value = 0.3 self.robot.forward(self.linear_velocity_value) time.sleep(0.7) self.robot.stop() return self.linear_velocity_value = 0.2 self.t_unit_distance = 3.7 self.robot.forward(self.linear_velocity_value) time.sleep(distance * self.t_unit_distance) self.robot.stop() def turn(self, direction, degrees): """Turn the bot either 90 or 180 degrees""" self.anticlockwise_turn_time = 0.35 self.clockwise_turn_time = 0.35 self.turn_velocity_value = 0.2 if (degrees == 180): self.turn_time = 2 * self.turn_time if (direction == "clockwise"): self.robot.right(self.turn_velocity_value) time.sleep(self.clockwise_turn_time) elif (direction == "anticlockwise"): self.robot.left(self.turn_velocity_value) time.sleep(self.anticlockwise_turn_time) self.robot.stop() def move_to_start(self, pitstop, startpoint, startnum): """Move the bot from pitstop to startpoint.""" self.pitstop = pitstop self.startpoint = startpoint delta_x = abs(startpoint[0] - pitstop[0]) delta_y = abs(startpoint[1] - pitstop[1]) if (startnum == 1): self.turn("anticlockwise", 90) self.move_forward(delta_y) self.turn("clockwise", 90) self.move_forward(delta_x) self.turn("clockwise", 90) #self.move_forward(self, 0, 1) self.startpoint.append(4.71) #radians elif (startnum == 2): self.move_forward(delta_x) self.turn("anticlockwise", 90) self.move_forward(delta_y) self.turn("clockwise", 90) #self.move_forward(self, 0, 1) self.startpoint.append(0) #radians elif (startnum == 3): self.move_forward(delta_x) self.turn("clockwise", 90) self.move_forward(delta_y) self.turn("clockwise", 90) #self.move_forward(self, 0, 1) self.startpoint.append(3.14) #radians self.robot.stop() def sprint(self, baton_callback, pathpoints_callback, start_num): """Navigate through the track.""" # Debugging : print(f'Hello from sprint with start_num {start_num}') self.baton_callback = baton_callback self.pathpoints_callback = pathpoints_callback self.start_num = start_num self.follow_road({'new': self.camera.value}) self.camera.observe(self.follow_road, names='value')
print(device) # Instanciate camera with defined size. camwidth, camheight = 320, 240 #224,224 cam = Camera.instance(width=camwidth, height=camheight) try: print("Camspecs are to specifications: ", ((cam.width == camwidth) and (cam.height == camheight))) print(cam.width, cam.height) except NameError: pass # Instanciate Camera robot = Robot() robot.stop( ) # Sometimes the robot start driving when instantiated, so this is in case it does this. def geometric_average(l: list): ''' This function returns the geometric average of the given list ''' return (sum(np.array(l)**2) / len(l))**(1 / 2) # We predefine the general constants used later for efficiency in the future cells. total = [] blocking_threshold = 0.85 # ## OF Setup
def main_loop(): global led_power led_power = max_led_power # Get access to the webcam. The method is different depending on if this is running on a laptop or a Jetson Nano. if running_on_jetson_nano(): # Accessing the camera with OpenCV on a Jetson Nano requires gstreamer with a custom gstreamer source string video_capture = cv2.VideoCapture(get_jetson_gstreamer_source(), cv2.CAP_GSTREAMER) else: # Accessing the camera with OpenCV on a laptop just requires passing in the number of the webcam (usually 0) # Note: You can pass in a filename instead if you want to process a video file instead of a live camera stream video_capture = cv2.VideoCapture(0) robot = Robot() print ("beginning Jetbot face recognition loop") noface_count = 0 try: while True: # Grab a single frame of video ret, frame = video_capture.read() # Resize frame of video to 1/4 size for faster face recognition processing small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25) # Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses) rgb_small_frame = small_frame[:, :, ::-1] # Find all the face locations and face encodings in the current frame of video face_locations = face_recognition.face_locations(rgb_small_frame) #face_encodings = face_recognition.face_encodings(rgb_small_frame, face_locations) # Grab the image of the the face from the current frame of video if (len(face_locations) > 0): noface_count = 0 max_width = 0 closest_face_left = 0 closest_face_right = 0 for location in face_locations: top, right, bottom, left = location width = right - left if (width > max_width): max_width = width closest_face_left = left closest_face_right = right error = closest_face_left + max_width / 2 - view_position_center if (abs(error) > position_tolerance): if (error > 0): x = threading.Thread(target=swivel_for_time, args=(robot,-swivel_speed,swivel_duration,)) # was robot.left(speed=swivel_speed) else: x = threading.Thread(target=swivel_for_time, args=(robot,swivel_speed,.1,)) # was robot.right(speed=swivel_speed) x.start() #time.sleep(.1) else: robot.stop() with open("/home/jetbot/jethead-stats.txt",'w',encoding = 'utf-8') as f: f.write("{} {} {}".format(closest_face_left, closest_face_right, error)) toggle_green_led(robot) # show heartbeat with red led else: robot.stop() noface_count += 1 with open("/home/jetbot/jethead-stats.txt",'w',encoding = 'utf-8') as f: f.write("{}".format(noface_count)) toggle_red_led(robot) # show heartbeat with red led except: with open("/home/jetbot/jethead-stats.txt",'w',encoding = 'utf-8') as f: f.write("crashed") # Release handle to the webcam print ("cleaning up after jetbot") video_capture.release() cv2.destroyAllWindows()
class VideoCamera(object): 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() def __del__(self): self.cap.release() self.robot.stop() def set_seg_model(self): self.segNet = jetson.inference.segNet("fcn-resnet18-sun") self.segNet.SetOverlayAlpha(150) self.buffers = segmentationBuffers(self.segNet) def set_detect_model(self): self.detectNet = jetson.inference.detectNet("ssd-mobilenet-v2") def get_seg(self): img = jetson.utils.cudaFromNumpy(self.frame) self.buffers.Alloc(img.shape, img.format) self.segNet.Process(img, ignore_class="toilet") self.segNet.Overlay(self.buffers.overlay, filter_mode="linear") self.segNet.Mask(self.buffers.mask, filter_mode="linear") self.img_rander = jetson.utils.cudaToNumpy(self.buffers.overlay) self.mask_rander = jetson.utils.cudaToNumpy(self.buffers.mask) self.floor_rander = self.mask_rander[:,:,1].copy() self.floor_rander[self.floor_rander[:,:]!=128] = 0 self.floor_rander[self.floor_rander[:,:]==128] = 1 self.floor_rander = self.floor_rander.reshape((120,320)) self.floor_rander[:,:160] = mask_roi(self.floor_rander[:,:160], self.roi) self.floor_rander[:,160:] = mask_roi(self.floor_rander[:,160:], self.roi) self.get_score() def get_detect(self): img = jetson.utils.cudaFromNumpy(self.frame) detections = self.detectNet.Detect(img, overlay="box,labels,conf") self.img_detect_rander = jetson.utils.cudaToNumpy(img) def get_score(self): self.n = np.sum(self.floor_rander) self.n_left = np.sum(self.floor_rander[:,:80]) self.n_right = np.sum(self.floor_rander[:,240:]) def update_jetbot(self): if self.avoidance_status and self.cruise_status: if self.n > 6000: self.direction = "Straight" self.robot.set_motors(self.pw*self.left_power, self.pw*self.right_power) elif self.n < 2000: self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.n_right >= self.n_left + 100: self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) elif self.n_left > self.n_right + 100: self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) else: self.direction = "Unknown" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) elif self.cruise_status: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": self.direction = "Straight" self.robot.set_motors(self.pw_c*self.left_power, self.pw_c*self.right_power) elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() elif self.avoidance_status: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": if self.n > 6000: self.direction = "Straight" self.robot.set_motors(self.pw_c*self.left_power, self.pw_c*self.right_power) else: self.direction = "stop with avoidance" self.robot.stop() self.move_arrow = "stop" elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() else: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": self.direction = "Straight" self.robot.set_motors(self.pw*self.left_power, self.pw*self.right_power) elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() self.move_arrow = "stop" def get_frame(self): image = self.img_rander detect_img = self.img_detect_rander text = f'floor: {self.n:0.2f}, left_n: {self.n_left}, right_n: {self.n_right}, direction: {self.direction}' image = cv2.putText(image, text, (5,15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA) ret, jpeg = cv2.imencode('.jpg', image) # roi visualization tmp = cv2.resize(self.floor_rander, dsize=(0, 0), fx=2, fy=2, interpolation=cv2.INTER_LINEAR) tmp = np.stack((255*tmp,)*3,axis = 2) result = np.concatenate((detect_img, image, tmp, ),axis=0) self.result = result ret, jpeg = cv2.imencode('.jpg', result) return jpeg.tobytes() def update(self): while True: start = time.time() (self.grabbed, self.frame) = self.cap.read() # segmentation self.get_seg() # detection self.get_detect() # update jetbot self.update_jetbot() end = time.time() - start if end < 0.1: time.sleep(0.1-end) print("time :", end, ",\ttotal: ", time.time() - start) # check time
print("Jetbot is moving foward!") elif mode == 'b': move.backward(0.5) print("Jetbot is moving backward!") elif mode == 'l': move.left(0.5) print("Jetbot is turning left!") elif mode == 'r': move.right(0.5) print("Jetbot is turning right!") elif mode == 's': move.stop() elif mode == 'c': joy = xbox.Joystick() while True: if (joy.Y()): move.forward(0.5) print("Jetbot is moving foward!") elif (joy.A()): move.backward(0.5) print("Jetbot is moving backward!") elif (joy.X()): move.left(1) move.stop() move.forward(0.5) print("Jetbot is turning left!")