diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 5fa7cd17..43b07905 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -155,17 +155,23 @@ struct IncWaveHeightPrivate auto sec_nsec = gz::math::durationToSecNsec(current_time_); response->heights.resize(request->points.size()); - double relative_time = request->relative_time; bool use_buoy_origin = request->use_buoy_origin; for (std::size_t idx = 0U; idx < request->points.size(); ++idx) { + double t{0.0}; + if (request->use_relative_time) { + t = SimTime + request->relative_time[idx]; + } else { + t = request->absolute_time[idx]; + } double x = request->points[idx].x; double y = request->points[idx].y; double eta{0.0}; gz::math::Quaternion q; - std::tie(eta, q) = compute_eta(x, y, SimTime + relative_time, use_buoy_origin); + std::tie(eta, q) = compute_eta(x, y, t, use_buoy_origin); - response->heights[idx].relative_time = relative_time; + // Note: absolute time is converted to relative (from current SimTime) + response->heights[idx].relative_time = t - SimTime; response->heights[idx].use_buoy_origin = use_buoy_origin; response->heights[idx].pose.header.stamp.sec = sec_nsec.first; response->heights[idx].pose.header.stamp.nanosec = sec_nsec.second; @@ -342,6 +348,7 @@ void IncWaveHeight::PreUpdate( double SimTime = std::chrono::duration(this->dataPtr->current_time_).count(); auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + // all fixed points from SDF computed at SimTime (relative_time = 0.0) latent_data.inc_wave_heights.sec = sec_nsec.first; latent_data.inc_wave_heights.nsec = sec_nsec.second; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index cff63be1..d5263d56 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -60,6 +60,7 @@ struct buoy_gazebo::LatentDataPublisherPrivate const buoy_gazebo::IncWaveHeightPoint & in, buoy_interfaces::msg::IncWaveHeight & out) { + // all fixed points from SDF computed at SimTime (relative_time = 0.0) out.relative_time = 0.0; out.use_buoy_origin = in.use_buoy_origin; out.pose.pose.position.x = in.x; @@ -239,6 +240,7 @@ void LatentDataPublisher::PostUpdate( std::size_t idx = 0U; for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) { + // all fixed points from SDF computed at SimTime (relative_time = 0.0) this->dataPtr->latent_data_.inc_wave_heights[idx].pose.header.stamp.sec = latent_data.inc_wave_heights.sec; this->dataPtr->latent_data_.inc_wave_heights[idx].pose.header.stamp.nanosec =