From f63a32124d3e38673cba939e6d3eb763ec3119b4 Mon Sep 17 00:00:00 2001
From: Sadanand Modak <sadanandm@utexas.edu>
Date: Fri, 16 Feb 2024 07:39:03 -0600
Subject: [PATCH] increased compare threshold, else wont work on jackal

---
 webviz.html | 24 ++++++++++++++----------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/webviz.html b/webviz.html
index 1514304..f13123b 100644
--- a/webviz.html
+++ b/webviz.html
@@ -431,6 +431,7 @@ <h1>UT AUTOmata Web Interface</h1>
 
       // Send waypoint.
       async gotoWaypoint(idx) {
+        console.log(`%cEntered gotoWaypoint for index ${idx}`, 'color: blue;');
         this.setNavGoal(this.waypoints[idx][0], this.waypoints[idx][1], this.waypoints[idx][2]);
         while (true) {
           if (resetNavGoals) {
@@ -445,13 +446,16 @@ <h1>UT AUTOmata Web Interface</h1>
       }
 
       async gotoWaypoints() {
+        console.log('%cEntered gotoWaypoints', 'color: green; font-weight: bold;');
         for (let i = 0; i < this.waypoints.length; i++) {
+          await this.gotoWaypoint(i);
           if (resetNavGoals) {
             break;
           }
-          await this.gotoWaypoint(i);
-          await new Promise(resolve => setTimeout(resolve, 10000)); // Wait for 10 seconds.
+          console.log(`%cAwaiting 15 seconds at waypoint ${i}`, 'color: gray;');
+          await new Promise(resolve => setTimeout(resolve, 15000)); // Wait for 15 seconds.
           if (i == this.waypoints.length - 1) {
+            console.log('%cLooping to start of waypoints array', 'color: pink; font-weight: bold;');
             i = -1;
           }
         }
@@ -508,14 +512,14 @@ <h1>UT AUTOmata Web Interface</h1>
     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
-      );
+      const positionThreshold = 0.7;
+      const orientationThreshold = 3.0;
+
+      x_ = Math.abs(x1 - x2);
+      y_ = Math.abs(y1 - y2);
+      theta_ = Math.abs(theta1 - theta2);
+      // console.log(`%cx_: ${x_}, y_: ${y_}, theta_: ${theta_}`, 'color: gray;');
+      return (x_ < positionThreshold && y_ < positionThreshold && theta_ < orientationThreshold);
     }
 
     function setLocationButton() {