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
