Objectives
Based on the previous lab, we now have a Bayes Filter skeleton. Now we are using BLE to control the robot car to spin in place in every 20 degrees. After obtaining the Range/Bearing data, we use the data for the update step of the Bayes Filter to predict the position of the robot.
1 The Real Robot Class
class RealRobot():
"""A class to interact with the real robot
"""
def __init__(self, commander, ble):
# Load world config
self.world_config = os.path.join(str(pathlib.Path(os.getcwd()).parent), "config", "world.yaml")
self.config_params = load_config_params(self.world_config)
# Commander to commuincate with the Plotter process
# Used by the Localization module to plot odom and belief
self.cmdr = commander
# ArtemisBLEController to communicate with the Robot
self.ble = ble
self.ToF_data = np.array([])
def get_pose(self):
"""Get robot pose based on odometry
Returns:
current_odom -- Odometry Pose (meters, meters, degrees)
"""
raise NotImplementedError("get_pose is not implemented")
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
"""
# should finish turning and return 18 data groups.
# ---- init storage ----
num_readings = 18
sensor_ranges = np.empty(num_readings)
sensor_bearings = np.empty(num_readings)
# ---- control the real robot and receive data ----
self.ble.start_notify(self.ble.uuid['RX_STRING'], self.rx_handler_ToF)
self.ble.send_command(CMD.PID_ROT_ON, "")
interval = 360 / num_readings
for count in range(num_readings):
self.ble.send_command(CMD.SET_TARGET_YAW, str(interval * count))
time.sleep(1.5)
self.ble.send_command(CMD.GET_DS, "")
s = self.ble.receive_string(self.ble.uuid['RX_STRING'])
self.ToF_data = np.append(self.ToF_data, s.split())
print("Data Group {}: {}".format(count + 1, s))
self.ble.send_command(CMD.PID_ROT_OFF, "")
self.ble.stop_notify(self.ble.uuid['RX_STRING'])
LOG.info("Stopped BLE Notifications")
self.ToF_data = self.ToF_data.reshape(-1, 3)
sensor_ranges = self.ToF_data[:, 2].reshape(-1, 1).astype('float') / 1000 # use the ToF sensor on the left side of car.
sensor_bearings = self.ToF_data[:, 0].reshape(-1, 1).astype('float')
return sensor_ranges, sensor_bearings
def rx_handler_ToF(self, uuid, byte_array):
if byte_array is not None:
each_ToF_data = byte_array.decode("utf-8").split()
self.ToF_data = np.append(self.ToF_data, each_ToF_data)
else:
LOG.error("INVALID byte_array")
2 Tune the PID Parameters
ble.send_command(CMD.SET_CAL_FAC, "1.05")
ble.send_command(CMD.SET_MAX_PWM, "145") # 145
ble.send_command(CMD.SET_CAL_DT, "1.09") # 1.3752, 1.2117, 1.3126. 1.23967, 1.1664, 1.1958, 1.2434
Kp_R = 12.00
Ki_R = 8.5
Kd_R = 0.5
ble.send_command(CMD.SET_P_ROT, str(Kp_R))
ble.send_command(CMD.SET_I_ROT, str(Ki_R))
ble.send_command(CMD.SET_D_ROT, str(Kd_R))
Here's the result on the lab floor surface.
3 Localize on the 4 Spots
Bottom Right Spot (5ft, -3ft, 0deg)
Belief: (8, 2, 1) in the map cell.
Ground Truth: (10, 1, 1)
Upper Right Spot (5ft, 3ft, 0deg)
Belief: (9, 6, 1) in the map cell.
Ground Truth: (10, 6, 1)
Bottom Left Spot (-3ft, -2ft, 0deg)
Belief: (4, 2, 1) in the map cell.
Ground Truth: (3, 2, 1)
Upper Central Spot (0ft, 3ft, 0deg)
Belief: (5, 7, 1) in the map cell.
Ground Truth: (5, 7, 1)
THE END