def get_altitude(self) -> Optional[float]: """ Gets the actual altitude (so not based on the simulated dvl) of the auv in its own reference frame by casting a ray to see where it intersects with terrain. This beam has length 100 so even if the altitude is larger than 100 it will still return 100. """ beam_length = 100 raytest_endpoint = 2 * Vec3([0, 0, beam_length]) world_frame_endpoint = vec3_local_to_world(self.get_position(), self.get_orientation(), raytest_endpoint) result = p.rayTest(self.get_position(), world_frame_endpoint) altitude = result[0] * beam_length if altitude >= 100: return None else: return altitude
def _check_ray_hits_robot(self, start_point: Vec3, endpoint: Vec3) -> bool: return PybulletAPI.rayTest( start_point, endpoint, object_id=self._robot.object_id)[0] < 1
def update(self, time: SimulationTime, dt: SimulationTime) -> None: altitudes = list() actual_altitude, current_velocity = self._get_real_values(dt) for i in range(4): # The raytest endpoint is twice as far as the range of the dvl, because this makes it possible to # smoothly interpolate between the transition between not having a lock and having a lock raytest_endpoint = 2 * self.beam_end_points[i] auv_frame_endpoint = vec3_local_to_world(self._sensor_position, self._sensor_orientation, raytest_endpoint) world_frame_endpoint = vec3_local_to_world(self._robot.get_position(), self._robot.get_orientation(), auv_frame_endpoint) result = PybulletAPI.rayTest(self._get_position(), world_frame_endpoint) altitudes.append(result[0] * 2 * MAXIMUM_ALTITUDE) # Change the color of the beam visualizer only if the state of the lock changes. if (self._previous_altitudes[i] >= MAXIMUM_ALTITUDE) != (altitudes[i] >= MAXIMUM_ALTITUDE): color = RED if altitudes[i] >= MAXIMUM_ALTITUDE else GREEN self.beamVisualizers[i].update(self._sensor_position, self.beam_end_points[i], color=color, frame_id=self._robot.object_id) self._queue = list() while self._next_sample_time <= time: interpolated_altitudes = list() for i in range(4): interpolated_altitudes.append(interpolate(x=self._next_sample_time.microseconds, x1=self._previous_update_time.microseconds, x2=time.microseconds, y1=self._previous_altitudes[i], y2=altitudes[i])) interpolated_bottom_lock = all(i < MAXIMUM_ALTITUDE for i in interpolated_altitudes) if interpolated_bottom_lock: interpolated_velocity = interpolate_vec(x=self._next_sample_time.microseconds, x1=self._previous_update_time.microseconds, x2=time.microseconds, y1=self._previous_velocity, y2=current_velocity) else: interpolated_velocity = Vec3((0, 0, 0)) self._queue.append( ( self._next_sample_time.seconds, { 'time': self._time_step.milliseconds, 'vx': interpolated_velocity[X], 'vy': interpolated_velocity[Y], 'vz': interpolated_velocity[Z], 'altitude': actual_altitude, 'velocity_valid': interpolated_bottom_lock, "format": "json_v1" } ) ) # The timestep of the DVL depends on the altitude (higher altitude is lower frequency) if actual_altitude is None: time_step_micros = MAXIMUM_TIME_STEP.microseconds else: time_step_micros = interpolate(actual_altitude, MINIMUM_ALTITUDE, MAXIMUM_ALTITUDE, MINIMUM_TIME_STEP.microseconds, MAXIMUM_TIME_STEP.microseconds) time_step_micros = clip(time_step_micros, MINIMUM_TIME_STEP.microseconds, MAXIMUM_TIME_STEP.microseconds) self._time_step = SimulationTime(int(time_step_micros)) self._next_sample_time += self._time_step self._previous_update_time = SimulationTime(time.microseconds) self._previous_altitudes = altitudes self._previous_velocity = current_velocity