diff --git a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp index 34ec0374..990179a8 100644 --- a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp +++ b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp @@ -26,7 +26,7 @@ namespace buoy_gazebo { -/// \brief Structure that holds shared ptr to incident wave object(s), and other data +/// \brief Structure that holds incident wave object(s), and other data struct IncWaveState { LinearIncidentWave Inc; diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 7db20c51..97751fd7 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -65,7 +65,6 @@ struct IncWaveHeightPrivate std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; std::atomic inc_wave_valid_{false}; - std::atomic got_new_request_{false}; std::thread thread_executor_spin_; std::atomic stop_{false}, paused_{true}; @@ -102,16 +101,15 @@ struct IncWaveHeightPrivate } std::tuple> compute_eta( - const double & _x, - const double & _y, + double & x, + double & y, const double & SimTime, const bool use_buoy_origin) { - double x = _x; - double y = _y; + // original x, y updated by reference if (use_buoy_origin) { - x += this->inc_wave_state.x; - y += this->inc_wave_state.y; + x += this->inc_wave_state.x; // x of buoy + y += this->inc_wave_state.y; // y of buoy } double deta_dx{0.0}, deta_dy{0.0}; @@ -154,39 +152,37 @@ struct IncWaveHeightPrivate double SimTime = std::chrono::duration(current_time_).count(); auto sec_nsec = gz::math::durationToSecNsec(current_time_); - this->got_new_request_ = true; - response->stamp.sec = sec_nsec.first; - response->stamp.nanosec = sec_nsec.second; response->heights.resize(request->points.size()); - this->inc_wave_heights.sec = sec_nsec.first; - this->inc_wave_heights.nsec = sec_nsec.second; - this->inc_wave_heights.points.clear(); - this->inc_wave_heights.points.resize(request->points.size()); + bool use_buoy_origin = request->use_buoy_origin; for (std::size_t idx = 0U; idx < request->points.size(); ++idx) { - bool use_buoy_origin = request->use_buoy_origin; + 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, use_buoy_origin); - - response->heights[idx].pose.position = request->points[idx]; - response->heights[idx].pose.position.z = eta; - - response->heights[idx].pose.orientation.x = q.X(); - response->heights[idx].pose.orientation.y = q.Y(); - response->heights[idx].pose.orientation.z = q.Z(); - response->heights[idx].pose.orientation.w = q.W(); - - this->inc_wave_heights.points[idx].use_buoy_origin = use_buoy_origin; - this->inc_wave_heights.points[idx].x = x; - this->inc_wave_heights.points[idx].y = y; - this->inc_wave_heights.points[idx].eta = eta; - this->inc_wave_heights.points[idx].qx = q.X(); - this->inc_wave_heights.points[idx].qy = q.Y(); - this->inc_wave_heights.points[idx].qz = q.Z(); - this->inc_wave_heights.points[idx].qw = q.W(); + // x, y updated in place to world coords + std::tie(eta, q) = compute_eta(x, y, t, use_buoy_origin); + + // Note: absolute time is converted to relative (from current SimTime) + response->heights[idx].relative_time = t - SimTime; + response->heights[idx].use_buoy_origin = false; // always return world coords + response->heights[idx].pose.header.stamp.sec = sec_nsec.first; + response->heights[idx].pose.header.stamp.nanosec = sec_nsec.second; + response->heights[idx].pose.pose.position.x = x; // in world coords + response->heights[idx].pose.pose.position.y = y; // in world coords + response->heights[idx].pose.pose.position.z = eta; // height above waterplane + + // normal vector (2D slope) of wave at point + response->heights[idx].pose.pose.orientation.x = q.X(); + response->heights[idx].pose.pose.orientation.y = q.Y(); + response->heights[idx].pose.pose.orientation.z = q.Z(); + response->heights[idx].pose.pose.orientation.w = q.W(); } data.unlock(); }; @@ -252,6 +248,7 @@ void IncWaveHeight::Configure( } } } + this->dataPtr->inc_wave_heights.points.clear(); bool first = true; sdf::ElementPtr e{nullptr}; @@ -276,8 +273,6 @@ void IncWaveHeight::Configure( continue; } this->dataPtr->inc_wave_heights.points.push_back(pt); - - this->dataPtr->got_new_request_ = true; } } else { gzerr << "[ROS 2 Incident Wave Height] Could not parse input points from SDF" << std::endl; @@ -318,6 +313,16 @@ void IncWaveHeight::PreUpdate( return; } + // nothing to do + if (this->dataPtr->inc_wave_heights.points.size() == 0U) { + return; + } + + // can't process wave heights + if (!this->dataPtr->inc_wave_valid_) { + return; + } + buoy_gazebo::LatentData latent_data; if (_ecm.EntityHasComponentType( this->dataPtr->entity, @@ -329,50 +334,37 @@ void IncWaveHeight::PreUpdate( latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); } - // low prio data access - std::unique_lock low(this->dataPtr->low_prio_mutex_); - std::unique_lock next(this->dataPtr->next_access_mutex_); - std::unique_lock data(this->dataPtr->data_mutex_); - next.unlock(); - - if (!this->dataPtr->inc_wave_valid_) { - data.unlock(); - return; - } - - if (this->dataPtr->got_new_request_) { - latent_data.inc_wave_heights = this->dataPtr->inc_wave_heights; - this->dataPtr->got_new_request_ = false; - } - - if (latent_data.inc_wave_heights.points.size() == 0U) { - data.unlock(); - return; - } - 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; - latent_data.inc_wave_heights.valid = this->dataPtr->inc_wave_valid_; + latent_data.inc_wave_heights.points.resize(this->dataPtr->inc_wave_heights.points.size()); std::size_t idx = 0U; for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) { - double use_buoy_origin = latent_data.inc_wave_heights.points[idx].use_buoy_origin; - double x = latent_data.inc_wave_heights.points[idx].x; - double y = latent_data.inc_wave_heights.points[idx].y; + double use_buoy_origin = this->dataPtr->inc_wave_heights.points[idx].use_buoy_origin; + double x = this->dataPtr->inc_wave_heights.points[idx].x; + double y = this->dataPtr->inc_wave_heights.points[idx].y; double eta{0.0}; gz::math::Quaternion q; + // x, y updated in place to world coords std::tie(eta, q) = this->dataPtr->compute_eta(x, y, SimTime, use_buoy_origin); - latent_data.inc_wave_heights.points[idx].eta = eta; + // always report in world coords + latent_data.inc_wave_heights.points[idx].use_buoy_origin = false; + + latent_data.inc_wave_heights.points[idx].x = x; // in world coords + latent_data.inc_wave_heights.points[idx].y = y; // in world coords + latent_data.inc_wave_heights.points[idx].eta = eta; // height above waterplane + + // normal vector (2D slope) of wave at point latent_data.inc_wave_heights.points[idx].qx = q.X(); latent_data.inc_wave_heights.points[idx].qy = q.Y(); latent_data.inc_wave_heights.points[idx].qz = q.Z(); latent_data.inc_wave_heights.points[idx].qw = q.W(); } - data.unlock(); _ecm.SetComponentData( this->dataPtr->entity, @@ -404,6 +396,7 @@ void IncWaveHeight::PostUpdate( if (this->dataPtr->IncWaveEntity == gz::sim::kNullEntity) { this->dataPtr->inc_wave_valid_ = false; + data.unlock(); return; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index ce41df3a..f8fd3cec 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -158,7 +158,7 @@ struct LatentData AirSpring lower_spring; ElectroHydraulic electro_hydraulic; WaveBody wave_body; - + double piston_friction_force_valid; double piston_friction_force; // Newtons diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 1210da57..49705430 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -60,14 +60,16 @@ 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.position.x = in.x; - out.pose.position.y = in.y; - out.pose.position.z = in.eta; - out.pose.orientation.x = in.qx; - out.pose.orientation.y = in.qy; - out.pose.orientation.z = in.qz; - out.pose.orientation.w = in.qw; + out.pose.pose.position.x = in.x; + out.pose.pose.position.y = in.y; + out.pose.pose.position.z = in.eta; + out.pose.pose.orientation.x = in.qx; + out.pose.pose.orientation.y = in.qy; + out.pose.pose.orientation.z = in.qz; + out.pose.pose.orientation.w = in.qw; } }; @@ -238,6 +240,11 @@ 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 = + latent_data.inc_wave_heights.nsec; this->dataPtr->copy_inc_wave_height( latent_data.inc_wave_heights.points[idx], this->dataPtr->latent_data_.inc_wave_heights[idx]);