Addresses #75 - Remove unessecary units conversion in TethysComms (#129)

This PR attempts to remove unnecessary unit conversions in tethys from angular velocity -> force -> angular velocity. Signed-off-by: Arjo Chakravarty <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
osrf · Jan 27, 2022 · a573a08 · a573a08
1 parent babce6a
commit a573a08
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 21 deletions.
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -98,6 +98,7 @@
         <fluid_density>1025</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>
         <velocity_control>true</velocity_control>
+        <use_angvel_cmd>true</use_angvel_cmd>
       </plugin>
       <plugin
         filename="ignition-gazebo-joint-position-controller-system"

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
@@ -412,21 +412,8 @@ void TethysCommPlugin::CommandCallback(
 
   // Thruster
   ignition::msgs::Double thrusterMsg;
-  // TODO(arjo):
-  // Conversion from angular velocity to force b/c thruster plugin takes force
-  // Maybe we should change that?
-  // https://github.com/osrf/lrauv/issues/75
   auto angVel = _msg.propomegaaction_();
-
-  // force = thrust_coefficient * fluid_density * omega ^ 2 *
-  //         propeller_diameter ^ 4
-  // These values are defined in the model's Thruster plugin's SDF
-  auto force = 0.004312328425753156 * this->oceanDensity * pow(0.2, 4) * angVel * angVel;
-  if (angVel < 0)
-  {
-    force *=-1;
-  }
-  thrusterMsg.set_data(force);
+  thrusterMsg.set_data(angVel);
   this->thrusterPub.Publish(thrusterMsg);
 
   // Mass shifter
@@ -495,8 +482,6 @@ void TethysCommPlugin::PostUpdate(
     int(std::chrono::duration_cast<std::chrono::nanoseconds>(
     _info.simTime).count()) - stateMsg.header().stamp().sec() * 1000000000);
 
-  // TODO(anyone) Maybe angular velocity should come from ThrusterPlugin
-  // Propeller angular velocity
   auto propAngVelComp =
     _ecm.Component<ignition::gazebo::components::JointVelocity>(thrusterJoint);
   if (propAngVelComp->Data().size() != 1)

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh
@@ -106,7 +106,7 @@ namespace tethys
 
     /// Topic to publish to for thruster
     private: std::string thrusterTopic
-      {"propeller_joint/cmd_pos"};
+      {"propeller_joint/cmd_vel"};
 
     /// Topic to publish to for rudder
     private: std::string rudderTopic

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -52,14 +52,12 @@ void commandVehicleForward(const std::string &_name)
   transport::Node node;
   auto pub =
       node.Advertise<msgs::Double>(
-          "/model/" + _name + "/joint/propeller_joint/cmd_thrust");
+          "/model/" + _name + "/joint/propeller_joint/cmd_vel");
 
   msgs::Double thrustCmd;
 
   // 300 rpm -> 31.42rads^-1
-  //   -> (31.42rads^-1)^2 * 0.004422 (thrust coeff) * 1000 (fluid density) 
-  //      * 0.2m ^ 4 (prop diameter) = 6.9857
-  thrustCmd.set_data(6.9857);
+  thrustCmd.set_data(31.42);
 
   int sleep{0};
   int maxSleep{30};