def update(self,time_delta):
T_sonar_map = self.robot.get_robot_map_transform()*self.robot.get_sonar_robot_transform()
# Get sonar x and y for center point of sonar in map frame.
#print(T_sonar_map)
#print(T_sonar_map.item(2))
#print(T_sonar_map.item(5))
x = T_sonar_map.item(2)
y = T_sonar_map.item(5)
if self.sparki.port == '':
# calculate sonar distance from map
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
# Send range finder measurement to RangeFinder class
#print(self.robot.sonar_distance)
#print(self.omap.log_odds)
rangefinder = RangeFinder(self.robot.sonar_distance, self.omap.log_odds, x, y)
log_odds = rangefinder.update(self.robot.sonar_distance, self.omap.log_odds, x, y)
self.omap.log_odds = log_odds
else:
# get current rangefinder reading
self.robot.sonar_distance = self.sparki.dist
# calculate servo setting
servo = int(self.robot.sonar_angle*180./math.pi)
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors()
# send command
self.sparki.send_command(left_speed,left_dir,right_speed,right_dir,servo)
# update robot position
self.robot.update(time_delta)
