Merge branch 'main' into hamilton8415/ehpto_switchlossfix

osrf · Oct 4, 2023 · f19e813 · f19e813
2 parents 355fe4a + c83a6ab
commit f19e813
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 1 deletion.
diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp
@@ -294,6 +294,34 @@ void LatentDataPublisher::PostUpdate(
   this->dataPtr->latent_data_.electro_hydraulic.battery_storage_power =
     latent_data.electro_hydraulic.battery_storage_power;
 
+  this->dataPtr->latent_data_.wave_body.pose.position.x =
+    latent_data.wave_body.pose.X();
+  this->dataPtr->latent_data_.wave_body.pose.position.y =
+    latent_data.wave_body.pose.Y();
+  this->dataPtr->latent_data_.wave_body.pose.position.z =
+    latent_data.wave_body.pose.Z();
+  this->dataPtr->latent_data_.wave_body.pose.orientation.x =
+    latent_data.wave_body.pose.Rot().X();
+  this->dataPtr->latent_data_.wave_body.pose.orientation.y =
+    latent_data.wave_body.pose.Rot().Y();
+  this->dataPtr->latent_data_.wave_body.pose.orientation.z =
+    latent_data.wave_body.pose.Rot().Z();
+  this->dataPtr->latent_data_.wave_body.pose.orientation.w =
+    latent_data.wave_body.pose.Rot().W();
+
+  this->dataPtr->latent_data_.wave_body.twist.linear.x =
+    latent_data.wave_body.twist.X();
+  this->dataPtr->latent_data_.wave_body.twist.linear.y =
+    latent_data.wave_body.twist.Y();
+  this->dataPtr->latent_data_.wave_body.twist.linear.z =
+    latent_data.wave_body.twist.Z();
+  this->dataPtr->latent_data_.wave_body.twist.angular.x =
+    latent_data.wave_body.twist.Roll();
+  this->dataPtr->latent_data_.wave_body.twist.angular.y =
+    latent_data.wave_body.twist.Pitch();
+  this->dataPtr->latent_data_.wave_body.twist.angular.z =
+    latent_data.wave_body.twist.Yaw();
+
   this->dataPtr->latent_data_.wave_body.buoyancy.force.x =
     latent_data.wave_body.buoyant_force.X();
   this->dataPtr->latent_data_.wave_body.buoyancy.force.y =
@@ -306,6 +334,9 @@ void LatentDataPublisher::PostUpdate(
     latent_data.wave_body.buoyant_moment.Y();
   this->dataPtr->latent_data_.wave_body.buoyancy.torque.z =
     latent_data.wave_body.buoyant_moment.Z();
+  this->dataPtr->latent_data_.wave_body.buoyancy_total_power =
+    latent_data.wave_body.buoyancy_total_power;
+
   this->dataPtr->latent_data_.wave_body.radiation.force.x =
     latent_data.wave_body.radiation_force.X();
   this->dataPtr->latent_data_.wave_body.radiation.force.y =
@@ -318,6 +349,9 @@ void LatentDataPublisher::PostUpdate(
     latent_data.wave_body.radiation_moment.Y();
   this->dataPtr->latent_data_.wave_body.radiation.torque.z =
     latent_data.wave_body.radiation_moment.Z();
+  this->dataPtr->latent_data_.wave_body.radiation_total_power =
+    latent_data.wave_body.radiation_total_power;
+
   this->dataPtr->latent_data_.wave_body.excitation.force.x =
     latent_data.wave_body.exciting_force.X();
   this->dataPtr->latent_data_.wave_body.excitation.force.y =
@@ -330,11 +364,13 @@ void LatentDataPublisher::PostUpdate(
     latent_data.wave_body.exciting_moment.Y();
   this->dataPtr->latent_data_.wave_body.excitation.torque.z =
     latent_data.wave_body.exciting_moment.Z();
+  this->dataPtr->latent_data_.wave_body.excitation_total_power =
+    latent_data.wave_body.excitation_total_power;
 
   this->dataPtr->latent_data_.piston_friction_force =
     latent_data.piston_friction_force;
 
-  // TODO(andermi) fill in other stuff
+  this->dataPtr->data_valid_ = latent_data.valid();
 
   this->dataPtr->data_valid_ = latent_data.valid();
 

diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp
@@ -362,12 +362,20 @@ void WaveBodyInteractions::PreUpdate(
   }
 
   latent_data.wave_body.valid = true;
+  latent_data.wave_body.pose = w_Pose_p;
+  latent_data.wave_body.twist =
+    gz::math::Pose3<double>(
+    w_xdot_p.X(), w_xdot_p.Y(), w_xdot_p.Z(),
+    w_omega_p.X(), w_omega_p.Y(), w_omega_p.Z());
   latent_data.wave_body.buoyant_force = w_FBp;
   latent_data.wave_body.buoyant_moment = w_MBp;
+  latent_data.wave_body.buoyancy_total_power = w_FBp.Dot(w_xdot_p) + w_MBp.Dot(w_omega_p);
   latent_data.wave_body.radiation_force = w_FRp;
   latent_data.wave_body.radiation_moment = w_MRp;
+  latent_data.wave_body.radiation_total_power = w_FRp.Dot(w_xdot_p) + w_MRp.Dot(w_omega_p);
   latent_data.wave_body.exciting_force = w_FEp;
   latent_data.wave_body.exciting_moment = w_MEp;
+  latent_data.wave_body.excitation_total_power = w_FEp.Dot(w_xdot_p) + w_MEp.Dot(w_omega_p);
 
   _ecm.SetComponentData<buoy_gazebo::components::LatentData>(
     this->dataPtr->model.Entity(),