Skip to content

Commit

Permalink
Merge branch 'main' into andermi/latent_data
Browse files Browse the repository at this point in the history 
Signed-off-by: Michael Anderson <[email protected]>
  • Loading branch information
@andermi
andermi committed Sep 7, 2023
2 parents fd00588 + d846f5d commit b959789
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 71 deletions.
2 changes: 1 addition & 1 deletion buoy_gazebo/src/IncidentWaves/IncWaveState.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
117 changes: 55 additions & 62 deletions buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ struct IncWaveHeightPrivate

std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_;
std::atomic<bool> inc_wave_valid_{false};
std::atomic<bool> got_new_request_{false};

std::thread thread_executor_spin_;
std::atomic<bool> stop_{false}, paused_{true};
Expand Down Expand Up @@ -102,16 +101,15 @@ struct IncWaveHeightPrivate
}

std::tuple<double, gz::math::Quaternion<double>> 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};
Expand Down Expand Up @@ -154,39 +152,37 @@ struct IncWaveHeightPrivate
double SimTime = std::chrono::duration<double>(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<double> 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();
};
Expand Down Expand Up @@ -252,6 +248,7 @@ void IncWaveHeight::Configure(
}
}
}

this->dataPtr->inc_wave_heights.points.clear();
bool first = true;
sdf::ElementPtr e{nullptr};
Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand All @@ -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<double>(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<double> 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<buoy_gazebo::components::LatentData>(
this->dataPtr->entity,
Expand Down Expand Up @@ -404,6 +396,7 @@ void IncWaveHeight::PostUpdate(

if (this->dataPtr->IncWaveEntity == gz::sim::kNullEntity) {
this->dataPtr->inc_wave_valid_ = false;
data.unlock();
return;
}

Expand Down
2 changes: 1 addition & 1 deletion buoy_gazebo/src/LatentData/LatentData/LatentData.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
21 changes: 14 additions & 7 deletions buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
};

Expand Down Expand Up @@ -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]);
Expand Down

0 comments on commit b959789

Please sign in to comment.