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!")
