Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

waypoint nav interface added #5

Merged
merged 2 commits into from
Nov 3, 2023
Merged

waypoint nav interface added #5

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
db51966
waypoint nav interface added
sadanand1120 Nov 3, 2023
0390fc5
add a separate reset signal
sadanand1120 Nov 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
105 changes: 99 additions & 6 deletions webviz.html
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ <h1>UT AUTOmata Web Interface</h1>
Status: <label id="status">Disconnected</label><br>
<input type="button" id="setLocation" onclick="setLocationButton()" value="Set Pose">
<input type="button" id="setNavGoal" onclick="setNavGoalButton()" value="Set Nav Target">
<input type="button" id="setWaypoints" onclick="setWaypointsButton()" value="Set Waypoints">
<input type="button" id="resetNav" onclick="resetNavButton()" value="Reset Nav" />
Map:
<select id="map" value="GDC1" onchange="changeMap()">
<option value="GDC1">GDC1</option>
Expand Down Expand Up @@ -85,6 +87,8 @@ <h1>UT AUTOmata Web Interface</h1>
var mouseDown = false;
var setLocation = false;
var setNavGoal = false;
var setWaypoints = false;
var resetNavGoals = false;
var coords = document.getElementById("coords");
var mapList = document.getElementById("map");
var robotPose = undefined;
Expand Down Expand Up @@ -177,7 +181,7 @@ <h1>UT AUTOmata Web Interface</h1>
let end_of_str = false;
for (let i = 0; i < 32; i++) {
const c = this.readUint8();
if(c==0 || end_of_str) {
if (c == 0 || end_of_str) {
end_of_str = true;
} else {
s += String.fromCharCode(c)
Expand Down Expand Up @@ -309,6 +313,7 @@ <h1>UT AUTOmata Web Interface</h1>
// Constructor: set up callbacks.
constructor() {
this.socket = null;
this.waypoints = []; // list of tuples (x, y, theta) waypoints
// Set up callbacks.
this.callbacks = {
onopen: null,
Expand Down Expand Up @@ -412,6 +417,34 @@ <h1>UT AUTOmata Web Interface</h1>
}));
}

// Send waypoint.
async gotoWaypoint(idx) {
this.setNavGoal(this.waypoints[idx][0], this.waypoints[idx][1], this.waypoints[idx][2]);
while (true) {
if (resetNavGoals) {
break;
}
if (comparePoses([robotPose.x, robotPose.y, robotPose.theta], this.waypoints[idx])) {
console.log('Reached the waypoint');
break; // Exit the loop if the robot has reached the waypoint.
}
await new Promise(resolve => setTimeout(resolve, 100)); // Wait for 100 ms.
}
}

async gotoWaypoints() {
for (let i = 0; i < this.waypoints.length; i++) {
if (resetNavGoals) {
break;
}
await this.gotoWaypoint(i);
await new Promise(resolve => setTimeout(resolve, 10000)); // Wait for 10 seconds.
if (i == this.waypoints.length - 1) {
i = -1;
}
}
}

close() {
if (!this.socket) return;
this.socket.close();
Expand Down Expand Up @@ -460,18 +493,71 @@ <h1>UT AUTOmata Web Interface</h1>
xhr.send();
}

function comparePoses(pose1, pose2) {
const [x1, y1, theta1] = pose1;
const [x2, y2, theta2] = pose2;
const positionThreshold = 0.1;
const orientationThreshold = 0.05;

return (
Math.abs(x1 - x2) < positionThreshold &&
Math.abs(y1 - y2) < positionThreshold &&
Math.abs(theta1 - theta2) < orientationThreshold
);
}

function setLocationButton() {
setLocation = true;
setNavGoal = false;
resetNavGoals = false;
setWaypoints = false;
document.getElementById("resetNav").style = "";
document.getElementById("setLocation").style = "border-style:inset;";
}

function setNavGoalButton() {
setLocation = false;
setNavGoal = true;
resetNavGoals = false;
setWaypoints = false;
document.getElementById("resetNav").style = "";
document.getElementById("setNavGoal").style = "border-style:inset;";
}

function setWaypointsButton() {
if (!setWaypoints) {
setLocation = false;
setNavGoal = false;
resetNavGoals = false;
setWaypoints = true;
document.getElementById("resetNav").style = "";
document.getElementById("setWaypoints").style = "border-style:inset;";
} else {
setWaypoints = false;
document.getElementById("setWaypoints").style = "";
robot_interface.gotoWaypoints();
}
}

function resetNavButton() {
if (!resetNavGoals) {
setNavGoal = false;
setLocation = false;
resetNavGoals = true;
setWaypoints = false;
mouseDown = false;
robot_interface.waypoints = [];
document.getElementById("setLocation").style = "";
document.getElementById("setNavGoal").style = "";
document.getElementById("setWaypoints").style = "";
robot_interface.setNavGoal(1111, 1111, 1111); // all 1111 -> indicates reset of nav goals
sadanand1120 marked this conversation as resolved.
Show resolved Hide resolved
document.getElementById("resetNav").style = "border-style:inset;";
} else {
resetNavGoals = false;
document.getElementById("resetNav").style = "";
}
}

// Get the "true" mouse location, compensting for scroll, zoom, etc.
function getMouseLocation(event, element) {
var posx = 0;
Expand Down Expand Up @@ -516,7 +602,7 @@ <h1>UT AUTOmata Web Interface</h1>
const y_str = w.y.toFixed(3);
coords.innerHTML = `x=${x_str}, y=${y_str}`;
if (!mouseDown) return;
if (setLocation || setNavGoal) return;
if (setLocation || setNavGoal || setWaypoints) return;
origin.x = prev_origin.x + curMouseLoc.x - mouseDownLoc.x;
origin.y = prev_origin.y + curMouseLoc.y - mouseDownLoc.y;
};
Expand Down Expand Up @@ -549,6 +635,13 @@ <h1>UT AUTOmata Web Interface</h1>
let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
robot_interface.setInitialPose(w0.x, w0.y, a);
}
} else if (setWaypoints) {
// Set waypoints here.
if (robot_interface) {
let w0 = canvasToWorld(mouseDownLoc);
let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
robot_interface.waypoints.push([w0.x, w0.y, a]);
}
} else {
// Pan.
origin.x = prev_origin.x + loc.x - mouseDownLoc.x;
Expand Down Expand Up @@ -600,10 +693,10 @@ <h1>UT AUTOmata Web Interface</h1>
}

function drawTextAnnotation(text, x, y, color, size_em) {
ctx.font = `${Math.round(size_em*12)}pt Arial`;
ctx.font = `${Math.round(size_em * 12)}pt Arial`;
ctx.fillStyle = color;
ctx.strokeStyle = color;
const p = worldToCanvas({x: x, y: y})
const p = worldToCanvas({ x: x, y: y })
ctx.fillText(text, p.x, p.y);
}

Expand Down Expand Up @@ -792,7 +885,7 @@ <h1>UT AUTOmata Web Interface</h1>
if (visMsg == undefined) return;
visMsg.textAnnotations.forEach(function (m, i) {
let p = m.start;
if(i < visMsg.header.num_local_text_annotations) {
if (i < visMsg.header.num_local_text_annotations) {
p = robotToMap(p);
}
drawTextAnnotation(m.str, p.x, p.y, m.color, m.size_em);
Expand Down Expand Up @@ -964,4 +1057,4 @@ <h1>UT AUTOmata Web Interface</h1>
</script>
</body>

</html>
</html>
Loading