Skip to content

Commit

Permalink
integrate cv
Browse files Browse the repository at this point in the history
  • Loading branch information
mayarajan3 committed Nov 8, 2024
1 parent 77ac62e commit 6a73e15
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 161 deletions.
98 changes: 21 additions & 77 deletions extensions/src/doodlebot/Doodlebot.ts
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ import { followLine } from "./LineFollowing";
import { Command, DisplayKey, NetworkStatus, ReceivedCommand, SensorKey, command, display, endpoint, keyBySensor, motorCommandReceived, networkStatus, port, sensor } from "./enums";
import { base64ToInt32Array, makeWebsocket, Max32Int, testWebSocket } from "./utils";
import { line0, line1, line2, line3, line4, line5, line6, line7, line8 } from './Points';
import { LineDetector } from "./LineDetection";

export type Services = Awaited<ReturnType<typeof Doodlebot.getServices>>;
export type MotorStepRequest = {
Expand Down Expand Up @@ -634,87 +635,30 @@ export default class Doodlebot {
}



async followLine() {

let first = true;
const delay = 0.5;
const previousSpeed = 0.1;

const lineData = [line0, line1, line2, line3, line4, line5, line6, line7, line8];

const beforeLine = this.prependUntilTarget(lineData[0]);

let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], lineData[1], delay, previousSpeed, []);
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}


({ motorCommands, bezierPoints, line } = followLine(line, lineData[1], lineData[2], delay, previousSpeed, motorCommands));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[2], lineData[3], delay, previousSpeed, motorCommands));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[3], lineData[4], delay, previousSpeed, motorCommands));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[4], lineData[5], delay, previousSpeed, motorCommands));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[5], lineData[6], delay, previousSpeed, motorCommands));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
// "steps",
// { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) },
// { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) }
// );
console.log("command");
console.log(command);
}
const detector = new LineDetector(this.connection.ip);
let motorCommands, bezierPoints, line;
const intervalId = setInterval(async () => {
try {
const lineData = await detector.detectLine();
if (first) {
({ motorCommands, bezierPoints, line } = followLine(lineData, lineData, null, delay, previousSpeed, [], false, true));
} else {
({ motorCommands, bezierPoints, line } = followLine(line, lineData, null, delay, previousSpeed, motorCommands, false, false));
}
// Process the line data here
console.log(lineData);
} catch (error) {
console.error("Error detecting line:", error);
// Optionally stop polling if there's a consistent error
clearInterval(intervalId);
}
}, 1000 / 15); // 66.67 ms interval

}

Expand Down
122 changes: 61 additions & 61 deletions extensions/src/doodlebot/LineDetection.ts
Original file line number Diff line number Diff line change
@@ -1,61 +1,61 @@
// import { createCanvas, loadImage } from 'canvas';
// import { endpoint, port } from "./enums";

// export class LineDetector {
// private lastDetectedLine: number[][] = [];
// private isProcessing = false;
// private canvas: any;
// private ctx: any;

// constructor(private raspberryPiIp: string, private width = 640, private height = 480) {
// this.canvas = createCanvas(this.width, this.height);
// this.ctx = this.canvas.getContext('2d');
// }

// async detectLine(): Promise<number[][]> {
// if (this.isProcessing) return this.lastDetectedLine;
// this.isProcessing = true;

// try {
// const image = await loadImage(`http://${this.raspberryPiIp}:${port.camera}/${endpoint.video}`);
// this.ctx.drawImage(image, 0, 0, this.width, this.height);
// const imageData = this.ctx.getImageData(0, 0, this.width, this.height);
// const lineCoordinates = this.processImageData(imageData);

// if (lineCoordinates.length > 0) {
// this.lastDetectedLine = lineCoordinates;
// }

// return this.lastDetectedLine;
// } catch (error) {
// console.error('Error detecting line:', error);
// return this.lastDetectedLine;
// } finally {
// this.isProcessing = false;
// }
// }

// private processImageData(imageData: ImageData): number[][] {
// const lineCoordinates: number[][] = [];
// const threshold = 50;

// for (let y = 0; y < this.height; y++) {
// for (let x = 0; x < this.width; x++) {
// const index = (y * this.width + x) * 4;
// const r = imageData.data[index];
// const g = imageData.data[index + 1];
// const b = imageData.data[index + 2];
// if (r < threshold && g < threshold && b < threshold) {
// lineCoordinates.push([x, y]);
// }
// }
// }
// return lineCoordinates.sort((a, b) => a[1] - b[1]);
// }
// }

// export function createLineDetector(raspberryPiIp: string): () => Promise<number[][]> {
// const detector = new LineDetector(raspberryPiIp);
// return () => detector.detectLine();
// }
import { endpoint, port } from "./enums";
import cv from '@u4/opencv4nodejs';
import axios from 'axios';

export class LineDetector {
private lastDetectedLine: number[][] = [];
private isProcessing = false;

constructor(private raspberryPiIp: string, private width = 640, private height = 480) {}

async detectLine(): Promise<number[][]> {
if (this.isProcessing) return this.lastDetectedLine;
this.isProcessing = true;

try {
// Get image from endpoint
const response = await axios.get(
`http://${this.raspberryPiIp}:${port.camera}/${endpoint.video}`,
{ responseType: 'arraybuffer' }
);

// Convert response to cv Mat
const buffer = Buffer.from(response.data);
let mat = cv.imdecode(buffer);

// Resize if needed
if (mat.cols !== this.width || mat.rows !== this.height) {
mat = mat.resize(this.height, this.width);
}

// Convert to grayscale and apply threshold
const gray = mat.cvtColor(cv.COLOR_BGR2GRAY);
const blurred = gray.gaussianBlur(new cv.Size(5, 5), 0);
const thresh = blurred.threshold(0, 255, cv.THRESH_BINARY + cv.THRESH_OTSU);

// Find contours
const contours = thresh.findContours(cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE);
// Get the largest contour (assuming it's the line)
const sortedContours = contours.sort((c1, c2) => c2.area - c1.area);
if (sortedContours.length > 0) {
// Convert contour points to coordinate array
const coordinates = sortedContours[0].getPoints().map(point => [point.x, point.y]);
this.lastDetectedLine = coordinates;
}

return this.lastDetectedLine;
} catch (error) {
console.error('Error detecting line:', error);
return this.lastDetectedLine;
} finally {
this.isProcessing = false;
}
}
}

export function createLineDetector(raspberryPiIp: string): () => Promise<number[][]> {
const detector = new LineDetector(raspberryPiIp);
return () => detector.detectLine();
}
54 changes: 37 additions & 17 deletions extensions/src/doodlebot/LineFollowing.ts
Original file line number Diff line number Diff line change
Expand Up @@ -331,10 +331,14 @@ function prependUntilTarget(line) {
}


export function followLine(previousLine: Point[], pixels: Point[], next: Point[], delay: number, previousSpeed: number, previousCommands: Command[], first = false) {
let nextPoints = simplifyLine(next, epsilon, 0.1);
nextPoints = cutOffLineOnDistance(nextPoints.filter((point: Point) => point[1] < 370), maxDistance);
nextPoints = nextPoints.map(point => pixelToGroundCoordinates(point));
export function followLine(previousLine: Point[], pixels: Point[], next: Point[], delay: number, previousSpeed: number, previousCommands: Command[], test: Boolean, first = false) {

let nextPoints: Point[];
if (test) {
nextPoints = simplifyLine(next, epsilon, 0.1);
nextPoints = cutOffLineOnDistance(nextPoints.filter((point: Point) => point[1] < 370), maxDistance);
nextPoints = nextPoints.map(point => pixelToGroundCoordinates(point));
}

let worldPoints = simplifyLine(pixels, epsilon, 0.1);
worldPoints = cutOffLineOnDistance(worldPoints.filter((point: Point) => point[1] < 370), maxDistance);
Expand All @@ -347,13 +351,15 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
/* TESTING */
let multiplier = 1;
let distanceTest = 0.06 / multiplier;
try {
if (nextPoints && nextPoints.length > 20 && worldPoints.length > 20) {
let res = procrustes(worldPoints, nextPoints, 0.6);
distanceTest = res.distance
}

} catch (e) { }
if (test) {
try {
if (nextPoints && nextPoints.length > 20 && worldPoints.length > 20) {
let res = procrustes(worldPoints, nextPoints, 0.6);
distanceTest = res.distance
}

} catch (e) { }
}
/* TESTING */

let robotPosition = { x: 0, y: 0, angle: 0 };
Expand Down Expand Up @@ -418,8 +424,13 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
const spline = new Spline.default(ys, xs); // Switch x and y so we no overlapping 'x' values

// Find the end point for the Bezier curve
//const distance = previousSpeed*delay + lookahead;
/* TESTING */ const distance = distanceTest * 0.9; /* TESTING */
let distance: number;
if (test) {
distance = distanceTest * 0.9;
} else {
distance = previousSpeed*delay + lookahead;
}

const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01);
const x2 = findPointAtDistanceWithIncrements(spline, 0.001, distance);
const point1 = { x: spline.at(x1), y: x1 }
Expand All @@ -438,15 +449,24 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
};

// Find the start point for the Bezier curve -- account for camera latency
//const x3 = previousSpeed * delay;
/* TESTING */ const x3 = spline.xs[0]; /* TESTING */
let x3: number;
if (test) {
x3 = spline.xs[0];
} else {
x3 = previousSpeed * delay;
}
const point3 = { x: spline.at(x3), y: x3 }

// Find the x offset to correct
const reference1 = [spline.at(spline.xs[0]), 0] // First point should be very close to 0
const reference2 = [0, 0]
//let xOffset = reference1[0] - reference2[0];
/* TESTING */ let xOffset = 0; /* TESTING */

let xOffset: number;
if (test) {
xOffset = 0;
} else {
xOffset = reference1[0] - reference2[0];
}

// We want to correct the offset and direct the robot to a future point on the curve
// TODO: Add angle correction to the control points
Expand Down
12 changes: 6 additions & 6 deletions extensions/src/doodlebot/index.ts
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator

const beforeLine = this.prependUntilTarget(lineData[0]);

let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], lineData[1], delay, previousSpeed, [], true);
let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], lineData[1], delay, previousSpeed, [], true, true);
console.log("here");
for (const command of motorCommands) {
const { radius, angle } = command;
Expand All @@ -248,7 +248,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator
}


({ motorCommands, bezierPoints, line } = followLine(line, lineData[1], lineData[2], delay, previousSpeed, motorCommands));
({ motorCommands, bezierPoints, line } = followLine(line, lineData[1], lineData[2], delay, previousSpeed, motorCommands, true));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
Expand All @@ -260,7 +260,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[2], lineData[3], delay, previousSpeed, motorCommands));
({ motorCommands, bezierPoints, line } = followLine(line, lineData[2], lineData[3], delay, previousSpeed, motorCommands, true));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
Expand All @@ -272,7 +272,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[3], lineData[4], delay, previousSpeed, motorCommands));
({ motorCommands, bezierPoints, line } = followLine(line, lineData[3], lineData[4], delay, previousSpeed, motorCommands, true));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
Expand All @@ -284,7 +284,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[4], lineData[5], delay, previousSpeed, motorCommands));
({ motorCommands, bezierPoints, line } = followLine(line, lineData[4], lineData[5], delay, previousSpeed, motorCommands, true));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
Expand All @@ -296,7 +296,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator
console.log(command);
}

({ motorCommands, bezierPoints, line } = followLine(line, lineData[5], lineData[6], delay, previousSpeed, motorCommands));
({ motorCommands, bezierPoints, line } = followLine(line, lineData[5], lineData[6], delay, previousSpeed, motorCommands, true));
for (const command of motorCommands) {
const { radius, angle } = command;
// await this.motorCommand(
Expand Down

0 comments on commit 6a73e15

Please sign in to comment.