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 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)
net.GetNetworkFPS())) for detection in detections: index = detections[0].ClassID width = (detections[0].Width) location = (detections[0].Center[0]) #print("detection:") #print(index) #print(width) #print(location) if (index == 1): if (location > 800): print("Right") robot.right(robotSpeed) elif (location < 400): print("Left") robot.left(robotSpeed) else: print("Stop") robot.stop() else: print("Stop") robot.stop() # reset index = 0 width = 0 location = 0
print("largest_boxes_size:", largest_barrier_size) if len(largest_barrier_box) > 0: if len(target_boxes ) == 0 and largest_barrier_size <= 416 * 0.3: print("Target is not found, toward the barrier") target_boxes = np.array([largest_barrier_box]) barrier_center = detect_center(np.array([largest_barrier_box])) print("lagest_barrier_box_center:", barrier_center) if abs(barrier_center[0] ) < 0.2 and largest_barrier_size > 416 * 0.3: bypass_number = 3 light_on(LED_YELLOW) print("Bypassing barrier...") if barrier_center[0] <= 0: robot.right(0.4) else: robot.left(0.4) #robot.set_motors( # float(speed - turn_gain * barrier_center[0]), # float(speed + turn_gain * barrier_center[0]) #) time.sleep(0.2) robot.stop() continue if bypass_number >= 0: print("Bypassing barrier.", bypass_number) #robot.forward(0.3) robot.set_motors(0.42, 0.4) time.sleep(0.35)
if count % 5 == 0: start = time.time() text = model.predict(frame) end = time.time() write(str(end - start)) print(text) #text = model.predict(frame) if 'left' in text: robot.left(.5) time.sleep(.3) print('I detected left') count = 0 elif 'right' in text: print('I detected right') robot.right(.5) time.sleep(.3) count = 0 elif 'up' in text: pass else: print('I f****d up!') else: print('I am here') robot.forward(.65) cv2.putText(frame, "nothing", org=(50, 50),
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')
if len(human_data) >= 2: for a, b in list(itertools.combinations(human_data, 2)): if (abs(a.Center[0] - b.Center[0]) <= (a.Width / 2 + b.Width / 2)): position = int(a.Center[0] - width / 2) violate_num += 2 robot.stop() #Robot Motion if abs(position) < 200: robot.forward(0.4) elif position < 0: robot.left(0.3) elif position > 0: robot.right(0.3) #Display Camera Window img = jetson.utils.cudaToNumpy(img, width, height, 4) img = cv2.cvtColor(img.astype(np.uint8), cv2.COLOR_RGBA2BGR) #OverWrite the text about whether you have violated "social distance" cv2.putText(img, "Contact:{:4d}".format(violate_num), (0, 50), cv2.FONT_HERSHEY_PLAIN, 4, (0, 0, 255), 5, cv2.LINE_AA) cv2.imshow("img_test", img) cv2.waitKey(1) cv2.destroyAllWindows() except KeyboardInterrupt:
tasks = set(sys.argv[1:]) print("--- Initialization") from jetbot import Robot robot = Robot() print("--- Initialization done") if '1' in tasks: print('[Task 1] in 1s') time.sleep(1) robot.left(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.right(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.forward(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.backward(speed=0.3) time.sleep(1) robot.stop() print('[Task 1] done') if '2' in tasks: print('[Task 2] in 1s') time.sleep(1)
if value == 1: print("Start pressed") exit(0) if type & 0x02: axis = axis_map[number] if axis: #print("{}".format(axis)) if axis == "x": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue)) if (fvalue < 0): robot.left(speed=math.fabs(fvalue / 3)) elif (fvalue > 0): robot.right(speed=math.fabs(fvalue / 3)) else: robot.stop() if axis == "y": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue)) if (fvalue < 0): robot.forward(speed=math.fabs(fvalue / 3)) elif (fvalue > 0): robot.backward(speed=math.fabs(fvalue / 3)) else: robot.stop() if axis == "rx": fvalue = value / 32767
) if mode == 'f': move.forward(0.5) 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()):
if event.type == pygame.QUIT: # it will make exit the while loop robot.stop print("quit") run = False keys = pygame.key.get_pressed() if keys[pygame.K_LEFT]: print("left") robot.left(maxSpeed) if keys[pygame.K_RIGHT]: print("right") robot.right(maxSpeed) if keys[pygame.K_UP]: print("forward") robot.forward(maxSpeed) if keys[pygame.K_DOWN]: print("backward") robot.backward(maxSpeed) if keys[pygame.K_SPACE]: print("stop") robot.set_motors(0, 0) #pygame.display.update()
# ~ print("Jetbot is turning right!") # ~ elif (joy.Start()): # ~ move.stop() if (joy.leftY()): move.forward(0.5) print("Jetbot is moving foward!") elif (joy.rightY()): move.backward(0.5) print("Jetbot is moving backward!") elif (joy.leftX()): move.left(1) move.stop() move.forward(0.5) print("Jetbot is turning left!") elif (joy.rightX()): move.right(1) move.stop() move.forward(0.5) print("Jetbot is turning right!") elif (joy.Start()): move.stop() elif (joy.Back()): video.show_camera() # ~ image = widgets.Image(format = 'jpeg', width = 500, hight = 500) # ~ display(image) # ~ camera_link = trailets.dlink((camera, 'value'), (image, 'value'), transform = bgr8_to_jpeg) ## Ctrl + c to stop robot except KeyboardInterrupt: # Close serial connection