[sim] 2D Simulator Field of View Visualization + Enable LCM + Percept…

…ion Noise Code Reduction (#943)

simulators/nav/src/components/NavSimulator.vue

@@ -150,6 +150,9 @@ export default class NavSimulator extends Vue {
   @Getter
   private readonly simulatePercep!:boolean;
 
+  @Getter
+  private readonly enableLCM!:boolean;
+
   @Getter
   private readonly takeStep!:boolean;
 
@@ -345,7 +348,10 @@ export default class NavSimulator extends Vue {
   /* eslint-disable @typescript-eslint/no-explicit-any */
 
   /* Publish the given LCM message on the given channel. */
-  private publish(channel:string, payload:any):void {
+  private publish(channel:string, payload:any, odomOrObstacle:boolean):void {
+    if (!this.enableLCM && !odomOrObstacle) {
+      return;
+    }
     this.lcmBridge.publish(channel, payload);
   }
 
@@ -437,29 +443,29 @@ export default class NavSimulator extends Vue {
 
     /* Set up publishing LCMs */
     this.intervalLcmPublish = window.setInterval(() => {
-      this.publish('/auton', { type: 'AutonState', is_auton: this.autonOn });
+      this.publish('/auton', { type: 'AutonState', is_auton: this.autonOn }, false);
 
       if (this.simulateLoc) {
         const odom:any = Object.assign(this.currOdom, { type: 'Odometry' });
-        this.publish('/odometry', odom);
+        this.publish('/odometry', odom, true);
       }
 
       if (this.simulatePercep) {
         const obs:any = Object.assign(this.obstacleMessage, { type: 'Obstacle' });
-        this.publish('/obstacle', obs);
+        this.publish('/obstacle', obs, true);
 
         /* eslint no-magic-numbers: ["error", { "ignore": [0, 1] }] */
         const targetList:any = { targetList: this.targetList, type: 'TargetList' };
         targetList.targetList[0].type = 'Target';
         targetList.targetList[1].type = 'Target';
-        this.publish('/target_list', targetList);
+        this.publish('/target_list', targetList, false);
       }
 
       if (this.repeaterLoc !== null) {
-        this.publish('/rr_drop_complete', { type: 'RepeaterDrop' });
+        this.publish('/rr_drop_complete', { type: 'RepeaterDrop' }, false);
       }
 
-      this.publish('/radio', { type: 'RadioSignalStrength', signal_strength: this.radioStrength });
+      this.publish('/radio', { type: 'RadioSignalStrength', signal_strength: this.radioStrength }, false);
 
       const course:any = {
         type: 'Course',
@@ -474,10 +480,10 @@ export default class NavSimulator extends Vue {
         }))
       };
       course.hash = fnvPlus.fast1a52(JSON.stringify(course));
-      this.publish('/course', course);
+      this.publish('/course', course, false);
 
       const zedGimbalPos:any = Object.assign(this.zedGimbalPos, { type: 'ZedGimbalPosition' });
-      this.publish('/zed_gimbal_data', zedGimbalPos);
+      this.publish('/zed_gimbal_data', zedGimbalPos, false);
     }, TIME_INTERVAL_MILLI);
     /* eslint-enable @typescript-eslint/no-explicit-any */
   } /* created() */

simulators/nav/src/components/control_panel/DebugTools.vue

@@ -47,7 +47,7 @@
           <NumberInput
             :val.sync="fovAngleIn"
             :min="0"
-            :max="360"
+            :max="359"
           />
           <p>º</p>
         </div>
@@ -136,6 +136,9 @@ export default class DebugTools extends Vue {
   @Getter
   private readonly roverPath!:Odom[];
 
+   @Getter
+  private readonly FOVAreaPath!:Path2D;
+
   @Getter
   private readonly roverPathVisible!:boolean;
 

simulators/nav/src/components/control_panel/SimSettings.vue

@@ -45,7 +45,7 @@
           :precision="2"
           :min="0"
           :max="100"
-          :step="5.55"
+          :step="10"
         />
       </div>
       <div class="noiseGPS">
@@ -58,6 +58,20 @@
           :step="5"
         />
       </div>
+      <div class="enableFOVView">
+        <Checkbox
+          :on="enableFOVView"
+          name="FOV Visualization"
+          @clicked="flipEnableFOVView(!enableFOVView)"
+        />
+      </div>
+      <div class="enableLCM">
+        <Checkbox
+          :on="enableLCM"
+          name="Enable LCM"
+          @clicked="flipEnableLCM(!enableLCM)"
+        />
+      </div>
     </div>
   </fieldset>
 </template>
@@ -101,9 +115,16 @@ export default class SimSettings extends Vue {
   @Getter
   private readonly noiseGPSPercent!:number;
 
+  @Getter
+  private readonly enableLCM!:boolean;
+
+  @Getter
+  private readonly enableFOVView!:boolean;
+
   /************************************************************************************************
    * Vuex Mutations
    ************************************************************************************************/
+
   @Mutation
   private readonly setFieldSize!:(newFieldSize:number)=>void;
 
@@ -122,6 +143,12 @@ export default class SimSettings extends Vue {
   @Mutation
   private readonly setGPSNoisePercent!:(newNoisePercent:number)=>void;
 
+  @Mutation
+  private readonly flipEnableLCM!:(onOff:boolean)=>void;
+
+  @Mutation
+  private readonly flipEnableFOVView!:(onOff:boolean)=>void;
+
   /************************************************************************************************
    * Private Members
    ************************************************************************************************/

simulators/nav/src/components/field/Field.vue

@@ -181,6 +181,9 @@ export default class Field extends Vue {
   @Getter
   private readonly roverPath!:Odom[];
 
+  @Getter
+  private readonly FOVAreaPath!:Path2D;
+
   @Getter
   private readonly roverPathVisible!:boolean;
 
@@ -193,6 +196,9 @@ export default class Field extends Vue {
   @Getter
   private readonly zedGimbalPos!:ZedGimbalPosition;
 
+  @Getter
+  private readonly enableFOVView!:boolean;
+
   /************************************************************************************************
    * Vuex Mutations
    ************************************************************************************************/
@@ -214,6 +220,9 @@ export default class Field extends Vue {
   @Mutation
   private readonly pushToRoverPath!:(currLoc:Odom)=>void;
 
+  @Mutation
+  private readonly pushToFOVAreaPath!:(area:Path2D)=>void;
+
   @Mutation
   private readonly pushWaypoint!:(newWaypoint:Waypoint)=>void;
 
@@ -287,8 +296,9 @@ export default class Field extends Vue {
   /* Object for drawing rover on canvas. */
   private get canvasRover():CanvasRover {
     return new CanvasRover(this.currOdom, this.fieldCenterOdom, this.scale, this.roverPath,
-                           this.fieldOfViewOptions, this.roverPathVisible, this.pushToRoverPath,
-                           this.zedGimbalPos);
+                           this.FOVAreaPath, this.fieldOfViewOptions, this.roverPathVisible,
+                           this.pushToRoverPath, this.pushToFOVAreaPath, this.zedGimbalPos,
+                           this.enableFOVView);
   }
 
   /* Object for drawing waypoints on canvas. */

simulators/nav/src/components/field/rover.ts

@@ -61,18 +61,25 @@ export default class CanvasRover {
   /* rover's path */
   private path!:Odom[];
 
+  /* fov area */
+  private FOVAreaPath!:Path2D;
+
   /* Whether or not to draw the rover's path */
   private pathVisible!:boolean;
 
   /* Function to push a point to the rover's path */
   private pushToPath!:(currLoc:Odom)=>void;
 
+  private pushToFOVAreaPath!:(area:Path2D)=>void;
+
   /* scale of the canvas in pixels/meter */
   private scale!:number;
 
   /* Current position of the ZED Gimbal */
   private zedGimbalPos!:ZedGimbalPosition;
 
+  private enableFOVView!:boolean;
+
   /************************************************************************************************
    * Public Methods
    ************************************************************************************************/
@@ -82,19 +89,25 @@ export default class CanvasRover {
       canvasCent:Odom,
       scale:number, /* pixels/meter */
       path:Odom[],
+      FOVAreaPath:Path2D,
       fov:FieldOfViewOptions,
       pathVisible:boolean,
       pushToPath:(currLoc:Odom)=>void,
-      zedGimbalPos:ZedGimbalPosition
+      pushToFOVAreaPath:(area:Path2D)=>void,
+      zedGimbalPos:ZedGimbalPosition,
+      enableFOVView:boolean
   ) {
     this.currOdom = currOdom;
     this.canvasCent = canvasCent;
     this.scale = scale;
     this.path = path;
+    this.FOVAreaPath = FOVAreaPath;
     this.fov = fov;
     this.pathVisible = pathVisible;
     this.pushToPath = pushToPath;
+    this.pushToFOVAreaPath = pushToFOVAreaPath;
     this.zedGimbalPos = zedGimbalPos;
+    this.enableFOVView = enableFOVView;
 
     this.scaledEdgeOffset = EDGE_OFFSET * this.scale;
     this.scaledEboxLen = EBOX_LEN * this.scale;
@@ -122,6 +135,11 @@ export default class CanvasRover {
 
     const loc:Point2D = odomToCanvas(this.currOdom, this.canvasCent, canvas.height, this.scale);
 
+    /* Draw FOV */
+    if (this.fov.visible) {
+      this.drawFov(canvas);
+    }
+
     /* Draw path */
     if (this.pathVisible) {
       this.drawPath(canvas);
@@ -135,9 +153,6 @@ export default class CanvasRover {
     this.drawEbox();
     this.drawWheels();
     this.drawZed();
-    if (this.fov.visible) {
-      this.drawFov();
-    }
 
     this.ctx.rotate(-degToRad(this.currOdom.bearing_deg));
     this.ctx.translate(-loc.x, -loc.y);
@@ -162,30 +177,53 @@ export default class CanvasRover {
       x: 0,
       y: ((this.scaledRoverLen - this.scaledEboxLen) / 2) - this.scaledEdgeOffset
     };
+
     const startCorner:Point2D = {
       x: eboxLoc.x - (scaledEboxWdth / 2),
       y: eboxLoc.y - (this.scaledEboxLen / 2)
     };
+
     this.ctx.fillStyle = 'white';
     this.ctx.fillRect(startCorner.x, startCorner.y, scaledEboxWdth, this.scaledEboxLen);
   } /* drawEbox() */
 
   /* Draw the field of view on the canvas. */
-  private drawFov():void {
+  private drawFov(canvas):void {
+    if (this.path.length === 0) {
+      this.ctx.clearRect(0, 0, canvas.width, canvas.height);
+      return;
+    }
     const roverEyeLoc:Point2D = {
-      x: 0,
-      y: -this.scaledRoverLen / 2
+      x: this.scaledRoverLen / 2,
+      y: this.scaledRoverLen / 2
+    };
+    const currPos:Point2D = odomToCanvas(this.currOdom, this.canvasCent,
+                                         canvas.height, this.scale);
+    const currRelPos:Point2D = {
+      x: currPos.x + (roverEyeLoc.x * Math.sin(degToRad(this.currOdom.bearing_deg))),
+      y: currPos.y - (roverEyeLoc.y * Math.cos(degToRad(this.currOdom.bearing_deg)))
     };
+
     this.ctx.strokeStyle = 'black';
+    this.ctx.fillStyle = 'rgba(105, 82, 56, 0.6)';
     this.ctx.lineWidth = 3;
     this.ctx.beginPath();
-    this.ctx.moveTo(roverEyeLoc.x, roverEyeLoc.y);
-    this.ctx.arc(roverEyeLoc.x, roverEyeLoc.y, this.scaledFovDepth,
-                 compassToCanvasRad(degToRad((-this.fov.angle / 2) + this.zedGimbalPos.angle)),
-                 compassToCanvasRad(degToRad((this.fov.angle / 2) + this.zedGimbalPos.angle)),
-                 false);
-    this.ctx.lineTo(roverEyeLoc.x, roverEyeLoc.y);
-    this.ctx.stroke();
+
+    const region = new Path2D();
+    region.moveTo(currRelPos.x, currRelPos.y);
+    region.arc(currRelPos.x, currRelPos.y, this.scaledFovDepth,
+               compassToCanvasRad(degToRad((-this.fov.angle / 2) +
+               this.zedGimbalPos.angle + this.currOdom.bearing_deg)),
+               compassToCanvasRad(degToRad((this.fov.angle / 2) +
+               this.zedGimbalPos.angle + this.currOdom.bearing_deg)), false);
+    region.lineTo(currRelPos.x, currRelPos.y);
+    region.closePath();
+    this.ctx.stroke(region);
+    this.pushToFOVAreaPath(region);
+
+    if (this.enableFOVView) {
+      this.ctx.fill(this.FOVAreaPath);
+    }
   } /* drawFov() */
 
   /* Draw the rover's path. */

simulators/nav/src/components/field_items/FieldItems.vue

@@ -365,6 +365,9 @@ export default class FieldItems extends Vue {
   @Getter
   private readonly roverPath!:Odom[];
 
+  @Getter
+  private readonly FOVAreaPath!:Path2D[];
+
   @Getter
   private readonly waypoints!:Waypoint[];
 

simulators/nav/src/components/perception/target_detector.ts

@@ -29,13 +29,27 @@ import {
 } from '../../utils/constants';
 import { state } from '../../store/modules/simulatorState';
 
-// indZscore is from 0 to 19
-function getGaussianThres(indZscore):number {
+// state.simSettings.noisePercent - global
+function getGaussianThres():number {
   const sd = 0.2;
   const mu = 0.5;
-  const x = Zscores[indZscore] * sd;
-
-  return mu + x;
+  const percentFactor = 100.0;
+  const divisor = 10;
+  const factor = percentFactor / divisor; // 10
+  // check and invert the values of z scores
+  const neg = -1;
+  let indZscore = state.simSettings.noisePercent / factor;
+
+  const half = 5;
+  if (indZscore > half) {
+    indZscore = factor - indZscore;
+    const x = neg * Zscores[indZscore] * sd;
+    return mu + x;
+  }
+  else {
+    const x = Zscores[indZscore] * sd;
+    return mu + x;
+  }
 }
 
 /* Class that performs target dectection calculations. */
@@ -207,11 +221,7 @@ export default class TargetDetector {
 
     /* Special Case: guassian noise */
     const num:number = randnBm(0, 1, 1);
-    const divisor = 18.0;
-    const percentFactor = 100.0;
-    const factor = percentFactor / divisor;
-    const indThres = Math.round(state.simSettings.noisePercent / factor);
-    const thres = getGaussianThres(indThres);
+    const thres = getGaussianThres();
 
     if (num < thres) {
       post.isHidden = true;