Toggle TethysCommPlugin debug printout (#79)

* add plugin param to toggle TethysCommPlugin debug printout

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* style

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

lrauv_description/models/tethys_equipped/model.sdf

@@ -151,6 +151,7 @@
         <namespace>tethys</namespace>
         <command_topic>tethys/command_topic</command_topic>
         <state_topic>tethys/state_topic</state_topic>
+        <debug_printout>0</debug_printout>
       </plugin>
       <plugin
         filename="HydrodynamicsPlugin"

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -198,13 +198,17 @@ void TethysCommPlugin::Configure(
   {
     this->stateTopic = _sdf->Get<std::string>("state_topic");
   }
+  if (_sdf->HasElement("debug_printout"))
+  {
+    this->debugPrintout = _sdf->Get<bool>("debug_printout");
+  }
 
   // Initialize transport
   if (!this->node.Subscribe(this->commandTopic,
       &TethysCommPlugin::CommandCallback, this))
   {
-    ignerr << "Error subscribing to topic " << "[" << this->commandTopic << "]. "
-      << std::endl;
+    ignerr << "Error subscribing to topic " << "[" << this->commandTopic
+      << "]. " << std::endl;
     return;
   }
 
@@ -389,6 +393,7 @@ void TethysCommPlugin::CommandCallback(
   // Lazy timestamp conversion just for printing
   //if (std::chrono::seconds(int(floor(_msg.time_()))) - this->prevSubPrintTime
   //    > std::chrono::milliseconds(1000))
+  if (this->debugPrintout)
   {
     igndbg << "[" << this->ns << "] Received command: " << std::endl
       << _msg.DebugString() << std::endl;
@@ -599,7 +604,8 @@ void TethysCommPlugin::PostUpdate(
 
   this->statePub.Publish(stateMsg);
 
-  if (_info.simTime - this->prevPubPrintTime > std::chrono::milliseconds(1000))
+  if (this->debugPrintout &&
+    _info.simTime - this->prevPubPrintTime > std::chrono::milliseconds(1000))
   {
     igndbg << "[" << this->ns << "] Published state to " << this->stateTopic
       << " at time: " << stateMsg.header().stamp().sec()

lrauv_ignition_plugins/src/TethysCommPlugin.hh

@@ -92,6 +92,9 @@ namespace tethys
     /// \param[in] _ns Namespace to prepend to topic names
     private: void SetupControlTopics(const std::string &_ns);
 
+    /// Enable debug printout
+    private: bool debugPrintout = false;
+
     /// Namespace for topics.
     private: std::string ns{""};