Lab 11: Localization on the real robot

In this lab, we implement a bayes filter and run in in the environment from lab 9 to estimate the robot’s position on our sensor data.

Simulation

We start by running a simulation to test localization. Once we confirm this is working, we can move on to implementing localization on the real robot.

Odometry is in red, Ground truth in green, and belief in blue.

Reality

To do localization in reality, we need to be able to collect equidistant points in a circle. We can use the code that we used to get sensor readings in Lab 9:

void make_mapping(){
  if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
    myICM.readDMPdataFromFIFO(&imu_data);
  }
  while(!((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))){
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
  }
  get_roll_pitch_yaw(0);
  curr_setpoint = yaw_readings[0];
  record_tof_vals(num_datapoints, curr_setpoint);
  for (int j = 0; j < num_angles; j++){
    curr_setpoint -= angle_step;
    if (curr_setpoint > 180){
      curr_setpoint -= 360;
    }
    else if (curr_setpoint < -180) {
      curr_setpoint += 360;
    }
    float e = yaw_readings[i-1] - curr_setpoint;
    if (e > 180){
      e -= 360;
    }
    else if (e < -180) {
      e += 360;
    }
    // Make sure we're at the desired angle
    tx_estring_value.clear();
    tx_estring_value.append("GOING TO SETPOINT ");
    tx_estring_value.append(curr_setpoint);
    tx_characteristic_string.writeValue(tx_estring_value.c_str());  
    while (e > 3 || e < -3 ) {
      pid_angle(curr_setpoint);
      e = yaw_readings[i-1] - curr_setpoint;
      if (e > 180){
        e -= 360;
      }
      else if (e < -180) {
        e += 360;
    }
    }
    brake();
    tx_estring_value.clear();
    tx_estring_value.append("GETTING DATA");
    tx_characteristic_string.writeValue(tx_estring_value.c_str());  
    record_tof_vals(num_datapoints, curr_setpoint);  
  }

  tx_estring_value.clear();
  tx_estring_value.append("Transmitting");
  tx_characteristic_string.writeValue(tx_estring_value.c_str());  
  
  ////////////////// Start Transmition ////////////////////////////

  // Once we finish, we send the data over
  for (int j = 0; j < angle_idx; j++){
    tx_estring_value.clear();
    tx_estring_value.append(tof2_readings[j]);
    tx_estring_value.append(":");
    tx_estring_value.append(gyro_yaw_readings[j]);
    tx_characteristic_string.writeValue(tx_estring_value.c_str());
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
  }
  tx_estring_value.clear();
  tx_estring_value.append("Done");
  tx_characteristic_string.writeValue(tx_estring_value.c_str());          
  Serial.println("Done");
  mapping = false;
}

// Records Time of Flight values. In this case, we only need 1 value per angle
void record_tof_vals(int num_vals, int angle){
  for (int j = 0; j < num_vals; j++){
    while (!distanceSensor2.checkForDataReady()) {
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    while (!distanceSensor1.checkForDataReady()) {
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
      myICM.readDMPdataFromFIFO(&imu_data);
    }
    while(!((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))){
      if (myICM.status = ICM_20948_Stat_FIFOMoreDataAvail) {
        myICM.readDMPdataFromFIFO(&imu_data);
      }
    }
    time_stamps[angle_idx] = millis();
    get_roll_pitch_yaw(angle_idx);
    gyro_yaw_readings[angle_idx] = yaw_readings[angle_idx];
    tof1_readings[angle_idx] = distanceSensor1.getDistance();
    tof2_readings[angle_idx] = distanceSensor2.getDistance();
    setpoint_angle[angle_idx] = angle;

    distanceSensor1.clearInterrupt();
    distanceSensor2.clearInterrupt();
    angle_idx++;
    if (angle_idx >= TIME_ARR_SIZE){
      angle_idx = 0;
    }
  }
}

We program our perform_observation_loop to call this code with the MAP command.

    async def perform_observation_loop(self, rot_vel=120):
        """Perform the observation loop behavior on the real robot, where the robot does  
        a 360 degree turn in place while collecting equidistant (in the angular space) sensor
        readings, with the first sensor reading taken at the robot's current heading. 
        The number of sensor readings depends on "observations_count"(=18) defined in world.yaml.
        
        Keyword arguments:
            rot_vel -- (Optional) Angular Velocity for loop (degrees/second)
                        Do not remove this parameter from the function definition, even if you don't use it.
        Returns:
            sensor_ranges   -- A column numpy array of the range values (meters)
            sensor_bearings -- A column numpy array of the bearings at which the sensor readings were taken (degrees)
                               The bearing values are not used in the Localization module, so you may return a empty numpy array
        """
        sensor_ranges = []
        sensor_bearings = []

        def get_sensor_circle(uuid, byte_message):
            mess = ble.bytearray_to_string(byte_message)
            sep = mess.find(":")
            
            if sep != -1: 
                try: 
                    sensor_ranges.append(float(mess[:sep])/1000.0)
                    sensor_bearings.append(float(mess[sep+1:]))
                except:
                    print(f"Something went wrong with: {mess}")
            else: 
                print(mess)
                
        async def sleep_for_30_secs():
            # Data collection takes around 15 seconds, but we wait 30 to ensure we do not end before it is ready
            await asyncio.sleep(30) 
        
        ble.start_notify(ble.uuid['RX_STRING'], get_sensor_circle)
        ble.send_command(CMD.MAP, "18|1.8|0.01|0")
        await sleep_for_30_secs()
        ble.stop_notify(ble.uuid['RX_STRING'])                     
        return np.array(sensor_ranges)[np.newaxis].T, np.array(sensor_bearings)[np.newaxis].T

From there, we can observe the robot’s estimated position at various locations in the map.

At (0,0) and (-3,-2), the belief and truth were entirely overlapping. Both (0,0) and (-3,-2) have obvious features close by that allow the robot to guess its location more successfully. Unique wall configurations are close by which helps the location of the bot to be narrowed down.

For the rest of the points, the values were slightly off.

All the angles were off by 10 degrees. This may be the result of the angle I placed the bot at not being exactly 0. The time of flight might also be slightly angled on the bot.

The positions being slightly off could be the result of the sensor not rotating exactly on the dot since it is offset from the center of the car. This could also be due to less precise features helping narrow down the position or just general sensor noise.

Collaboration and Sources

I referred to Aidan McNay’s lab report when writing mine.