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)