[nav, teleop] urc auton testing fixes (#1150)

Co-authored-by: Cameron Tressler <[email protected]>
Co-authored-by: Ankith Udupa <[email protected]>

base_station/gui/src/components/AutonRoverMap.vue

@@ -30,7 +30,8 @@
     </l-map>
     <div class="controls">
       <div v-if="this.playbackEnabled">
-        <input type="range" min="0" :max='this.playbackLength-1' value ='0' v-model='playbackSlider'>
+        <input type="range" min="0" :max='this.playbackLength-1' value ='0' width="400px" v-model='playbackSlider'>
+        <!-- <p>State: {{ playbackState }}</p> -->
       </div>
       <div class="online">
         <label><input type="checkbox" v-model="online" />Online</label>
@@ -135,7 +136,11 @@ export default {
       playbackOdomLon: 'playbackOdomLon',
       playbackOdomBearing: 'playbackOdomBearing',
       playbackGpsBearing: 'playbackGpsBearing',
-      playbackTargetBearing: 'playbackTargetBearing'
+      playbackTargetBearing: 'playbackTargetBearing',
+      playbackProjectedLat: 'playbackProjectedLat',
+      playbackProjectedLon: 'playbackProjectedLon',
+      playbackProjectedType: 'playbackProjectedType',
+      playbackNavState: 'playbackNavState'
     }),
 
     odomLatLng: function () {
@@ -185,6 +190,7 @@ export default {
 
       playbackPath: [],
       playbackSlider: 0,
+      playbackState: '',
 
       options: {
         type: Object,
@@ -290,6 +296,11 @@ export default {
 
         this.targetBearingMarker.setRotationAngle(this.playbackTargetBearing[0])
         this.targetBearingMarker.setLatLng(latLng)
+
+        for (let i = 0; i < this.playbackProjectedLat[0].length && i < this.playbackProjectedLon[0].length; i++) {
+          this.projectedPoints.push(L.latLng(this.playbackProjectedLat[0][i], this.playbackProjectedLon[0][i]))
+        }
+        this.projectedPointsType = this.playbackProjectedType[0]
       }
     },
 
@@ -305,6 +316,11 @@ export default {
       this.targetBearingMarker.setRotationAngle(this.playbackTargetBearing[val])
       this.targetBearingMarker.setLatLng(latLng)
 
+      for (let i = 0; i < this.playbackProjectedLat[val].length && i < this.playbackProjectedLon[val].length; i++) {
+        this.projectedPoints.push(L.latLng(this.playbackProjectedLat[val][i], this.playbackProjectedLon[val][i]))
+      }
+      this.projectedPointsType = this.playbackProjectedType[val]
+
       let length_diff = val - this.playbackPath.length
 
       if (length_diff > 0) {
@@ -318,6 +334,8 @@ export default {
       else if (length_diff < 0) {
         this.playbackPath.splice(val, -1*length_diff)
       }
+
+      this.playbackState = this.playbackNavState[val]
     }
   },
 

base_station/gui/src/components/PlaybackAutonData.vue

@@ -20,7 +20,11 @@ export default {
       gps_bearing: [],
       target_bearing: [],
       waypoint_lat: [],
-      waypoint_lon: []
+      waypoint_lon: [],
+      projected_points_lats: [],
+      projected_points_lons: [],
+      projected_points_types: [],
+      nav_state: []
     }
   },
 
@@ -32,7 +36,11 @@ export default {
       setPlaybackOdomLon: 'setPlaybackOdomLon',
       setPlaybackOdomBearing: 'setPlaybackOdomBearing',
       setPlaybackGpsBearing: 'setPlaybackGpsBearing',
-      setPlaybackTargetBearing: 'setPlaybackTargetBearing'
+      setPlaybackTargetBearing: 'setPlaybackTargetBearing',
+      setPlaybackProjectedLat: 'setPlaybackProjectedLat',
+      setPlaybackProjectedLon: 'setPlaybackProjectedLon',
+      setPlaybackProjectedType: 'setPlaybackProjectedType',
+      setPlaybackNavState: 'setPlaybackNavState'
     }),
 
     upload_log: function() {
@@ -67,6 +75,12 @@ export default {
           let waypoint_lat_idx = 0
           let waypoint_lon_idx = 0
 
+          let projected_points_lat_idx = 0
+          let projected_points_lon_idx = 0
+          let projected_points_type_idx = 0
+
+          let nav_state_idx = 0
+
           for (let i = 0; i < parsed_data[0].length; i++) {
             switch (parsed_data[0][i]) {
               case 'Odom Degrees Lat':
@@ -96,6 +110,18 @@ export default {
               case 'First Waypoint Lon':
                 waypoint_lon_idx = i
                 break
+              case 'Projected Path Lat':
+                projected_points_lat_idx = i
+                break
+              case 'Projected Path Lon':
+                projected_points_lon_idx = i
+                break
+              case 'Projected Path Type':
+                projected_points_type_idx = i
+                break
+              case 'Nav State':
+                nav_state_idx = i
+                break
             }
           }
 
@@ -135,6 +161,17 @@ export default {
               this.waypoint_lat.push(waypoint_lat)
               this.waypoint_lon.push(waypoint_lon)
             }
+
+            this.projected_points_lats.push(parsed_data[i][projected_points_lat_idx].split('&').map((str) => {
+              return parseFloat(str)
+            }))
+            this.projected_points_lons.push(parsed_data[i][projected_points_lon_idx].split('&').map((str) => {
+              return parseFloat(str)
+            }))
+
+            this.projected_points_types.push(parsed_data[i][projected_points_type_idx])
+
+            this.nav_state.push(parsed_data[i][nav_state_idx])
           }
 
           for (let i = 0; i < this.waypoint_lat.length; i++) {
@@ -147,6 +184,10 @@ export default {
           this.setPlaybackOdomBearing(this.odom_bearing)
           this.setPlaybackGpsBearing(this.gps_bearing)
           this.setPlaybackTargetBearing(this.target_bearing)
+          this.setPlaybackProjectedLat(this.projected_points_lats)
+          this.setPlaybackProjectedLon(this.projected_points_lons)
+          this.setPlaybackProjectedType(this.projected_points_types)
+          this.setPlaybackNavState(this.nav_state)
 
           console.log("SUCCESSFULLY READ DATA!")
           this.setPlaybackEnabled(true)
@@ -159,7 +200,11 @@ export default {
           this.target_bearing = []
           this.waypoint_lat = []
           this.waypoint_lon = []
-        }) // end callback
+          this.projected_points_lats = []
+          this.projected_points_lons = []
+          this.projected_points_types = []
+          this.nav_state = []
+        }) // end callback for reader
 
         reader.readAsBinaryString(file)
       }

base_station/gui/src/components/SaveAutonData.vue

@@ -74,8 +74,15 @@ export default {
             id0: [],
             bearing1: [],
             distance1: [],
-            id1: []
+            id1: [],
 
+            current_projected_path_lat: [],
+            current_projected_path_lon: [],
+            current_projected_path_type: "",
+
+            projected_path_lat: [],
+            projected_path_lon: [],
+            projected_path_type: []
         }
     },
 
@@ -132,6 +139,20 @@ export default {
     },
 
     created: function() {
+
+        this.$parent.subscribe('/projected_points', (msg) => {
+            this.current_projected_path_lat = []
+            this.current_projected_path_lon = []
+
+            let new_projected_list = msg.points
+
+            for (let i = 0; i < msg.pattern_size; i++) {
+                this.current_projected_path_lat.push(msg.points[i].latitude_deg + msg.points[i].latitude_min/60)
+                this.current_projected_path_lon.push(msg.points[i].longitude_deg + msg.points[i].longitude_min/60)
+            }
+
+            this.current_projected_path_type = msg.path_type
+        })
  
         // upon closing, ask user if they want to close and then download
         window.addEventListener('beforeunload', (e) =>  {
@@ -213,6 +234,10 @@ export default {
                 this.distance1.push(this.TargetList[1].distance)
                 this.id1.push(this.TargetList[1].id)
 
+                this.projected_path_lat.push(this.current_projected_path_lat)
+                this.projected_path_lon.push(this.current_projected_path_lon)
+                this.projected_path_type.push(this.current_projected_path_type)
+
                 if (this.num_logs > (seconds_to_save / update_rate) + overflow_amt) {
                     this.num_logs -= overflow_amt
                     this.timestamp.splice(0, overflow_amt)
@@ -267,6 +292,10 @@ export default {
                     this.bearing1.splice(0,overflow_amt)
                     this.distance1.splice(0,overflow_amt)
                     this.id1.splice(0,overflow_amt)
+
+                    this.projected_path_lat.splice(0, overflow_amt)
+                    this.projected_path_lon.splice(0, overflow_amt)
+                    this.projected_path_type.splice(0, overflow_amt)
                 }
             }
         }, update_rate * 1000)
@@ -286,7 +315,7 @@ export default {
                 return
             }
 
-            var csv = 'Timestamp,Auton Enabled,Odom Degrees Lat,Odom Minutes Lat,Odom Degrees Lon,Odom Minutes Lon,Odom bearing,Odom speed,Accel X,Accel Y,Accel Z,Gyro X,Gyro Y,Gyro Z,Mag X,Mag Y,Mag Z,Roll,Pitch,Yaw,Calibration Sys,Calibration Gyro,Calibration Accel,Calibration Mag,IMU Bearing,GPS Degrees Lat,GPS Minutes Lat,GPS Degrees Lon,GPS Minutes Lon,GPS Bearing,GPS Speed,Target Bearing,Nav State,Waypoints Completed,Total Waypoints,First Waypoint Lat,First Waypoint Lon,Left Control,Right Control,Bearing0,Distance0,id0,Bearing1,Distance1,id1\n'
+            var csv = 'Timestamp,Auton Enabled,Odom Degrees Lat,Odom Minutes Lat,Odom Degrees Lon,Odom Minutes Lon,Odom bearing,Odom speed,Accel X,Accel Y,Accel Z,Gyro X,Gyro Y,Gyro Z,Mag X,Mag Y,Mag Z,Roll,Pitch,Yaw,Calibration Sys,Calibration Gyro,Calibration Accel,Calibration Mag,IMU Bearing,GPS Degrees Lat,GPS Minutes Lat,GPS Degrees Lon,GPS Minutes Lon,GPS Bearing,GPS Speed,Target Bearing,Nav State,Waypoints Completed,Total Waypoints,First Waypoint Lat,First Waypoint Lon,Left Control,Right Control,Bearing0,Distance0,id0,Bearing1,Distance1,id1,Projected Path Lat,Projected Path Lon,Projected Path Type\n'
 
             for (let i = 0; i < this.num_logs; i++) {
                 csv += this.timestamp[i] + ','
@@ -340,7 +369,19 @@ export default {
                 csv += this.id0[i] + ','
                 csv += this.bearing1[i] + ','
                 csv += this.distance1[i] + ','
-                csv += this.id1[i]
+                csv += this.id1[i] + ','
+
+                for (let j = 0; j < this.projected_path_lat[i].length; j++) {
+                    csv += this.projected_path_lat[i][j].toString() + '&'
+                }
+                csv += ','
+
+                for (let j = 0; j < this.projected_path_lon[i].length; j++) {
+                    csv += this.projected_path_lon[i][j].toString() + '&'
+                }
+                csv += ','
+
+                csv += this.projected_path_type[i]
 
                 csv += '\n'
             }
@@ -411,6 +452,10 @@ export default {
             this.bearing1 = []
             this.distance1 = []
             this.id1 = []
+
+            this.projected_path_lat = []
+            this.projected_path_lon = []
+            this.projected_path_type = []
         }
     }
 }

base_station/gui/src/store/modules/autonomy.js

@@ -16,7 +16,13 @@ const state = {
   playbackOdomLon: [],
   playbackOdomBearing: [],
   playbackGpsBearing: [],
-  playbackTargetBearing: []
+  playbackTargetBearing: [],
+
+  playbackProjectedLat: [],
+  playbackProjectedLon: [],
+  playbackProjectedType: [],
+
+  playbackNavState: []
 }
 
 // getters
@@ -35,6 +41,10 @@ const getters = {
   playbackOdomBearing: state => state.playbackOdomBearing,
   playbackGpsBearing: state => state.playbackGpsBearing,
   playbackTargetBearing: state => state.playbackTargetBearing,
+  playbackProjectedLat: state => state.playbackOdomLat,
+  playbackProjectedLon: state => state.playbackOdomLon,
+  playbackProjectedType: state => state.playbackOdomType,
+  playbackNavState: state => state.playbackNavState,
   playbackLength: state => state.playbackOdomLat.length
 }
 
@@ -90,6 +100,22 @@ const mutations = {
 
   setPlaybackTargetBearing (commit, bearing) {
     state.playbackTargetBearing = bearing
+  },
+
+  setPlaybackProjectedLat (commit, lat) {
+    state.playbackProjectedLat = lat
+  },
+
+  setPlaybackProjectedLon (commit, lon) {
+    state.playbackProjectedLon = lon
+  },
+
+  setPlaybackProjectedType (commit, type) {
+    state.playbackProjectedType = type
+  },
+
+  setPlaybackNavState (commit, nav_state) {
+    state.playbackNavState = nav_state
   }
 }
 

config/nav/config.json

@@ -6,6 +6,13 @@
 		"kD": 0.0
 	},
 
+	"driveBearingPid":
+	{
+		"kP": 0.05,
+		"kI": 0.0,
+		"kD": 0.0
+	},
+
 	"distancePid":
 	{
 		"kP": 0.2,

jetson/nav/environment.cpp

@@ -29,10 +29,18 @@ void Environment::setTargets(TargetList const& targets) {
         mLeftTargetBearing.push(mTargetLeft.get().bearing);
         mLeftTargetDistance.push(mTargetLeft.get().distance);
     }
+    else{
+        mLeftTargetDistance.reset();
+        mLeftTargetBearing.reset();
+    }
     if (mTargetRight.isValid()){
         mRightTargetBearing.push(mTargetRight.get().bearing);
         mRightTargetDistance.push(mTargetRight.get().distance);
     }
+    else{
+        mRightTargetDistance.reset();
+        mRightTargetBearing.reset();
+    }
 
 }