def get_sensor_position(self, sensor_id):
# front sensors are numbered from left to right, with `sensors_pitch` distance between them
# they are positioned in a line perpendicular to track direction and `sensors_dist` apart from robot's position
#calculate sensor position without considering heading
sensor_x = self.sensors_dist
sensor_y = ((self.n_sensors - 1)/2 - sensor_id) * self.sensors_pitch
#transform into polar coordinates
r, theta = coord.xy2polar(sensor_x, sensor_y)
#sum heading to theta to calculate sensor position in reference to robot position
new_theta = theta + self.heading
new_x, new_y = coord.polar2xy(r, new_theta)
#calculate final position in reference to track's origin
final_sensor_x = self.pos.x + new_x
final_sensor_y = self.pos.y - new_y #inverted sign due to inverted Y axis direction
return point.Point(final_sensor_x, final_sensor_y)
def calc_new_position(self):
x, y = coord.polar2xy(self.speed, self.heading)
delta = point.Point(x, -y)
self.set_pos(self.pos + delta)
