Parse ode_quiet physics parameter from SDFormat (#3194)

The ODE world-step solver has a tendency to spam the console
with LCP Internal Error messages. We added the `ode_quiet`
parameter to the `ODEPhysics::SetParam` C++ API to disable
these messages, but it would be more convenient to set
`ode_quiet` in an SDFormat world file. The PresetManager
does pass `ode_quiet` to the C++ API, but there is a casting
error. Since the ode_quiet element is not part of the SDFormat
spec, it is encoded as a string in the PresetManager.
Parsing it as a string using an sdf::Param object fixes it.
A test is added using world_step.world to confirm the fix.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

gazebo/physics/ode/ODEPhysics.cc

@@ -30,6 +30,8 @@
 #include <ignition/math/Vector3.hh>
 #include <ignition/common/Profiler.hh>
 
+#include <sdf/Param.hh>
+
 #include "gazebo/util/Diagnostics.hh"
 #include "gazebo/common/Assert.hh"
 #include "gazebo/common/Console.hh"
@@ -1557,7 +1559,25 @@ bool ODEPhysics::SetParam(const std::string &_key, const boost::any &_value)
     }
     else if (_key == "ode_quiet")
     {
-      bool odeQuiet = any_cast<bool>(_value);
+      bool odeQuiet;
+      try
+      {
+        odeQuiet = any_cast<bool>(_value);
+      }
+      catch(std::bad_any_cast &)
+      {
+        // If added to an SDFormat world file, this parameter will be
+        // encoded as a string, since it is not part of the SDFormat spec.
+        // In order to parse it, define an sdf::Param with type string
+        // and call Get<bool> to use libsdformat's existing logic for this.
+        // copied from sdformat's Param_TEST.cc
+        sdf::Param strParam("key", "string", "false", false, "description");
+        // cast to std::string and set the sdf::Param with this value
+        strParam.Set(any_cast<std::string>(_value));
+        // get the string value as bool
+        strParam.Get<bool>(odeQuiet);
+      }
+
       if (odeQuiet)
       {
         dSetMessageHandler(&dMessageQuiet);

test/regression/351_world_step.cc

@@ -35,4 +35,21 @@ TEST_F(Issue351Test, WorldStep)
 
   // Take 500 steps; it passes if it doesn't seg-fault
   world->Step(500);
+
+  // Confirm that ode_quiet has been set to true.
+  physics::PhysicsEnginePtr physics = world->Physics();
+  ASSERT_TRUE(physics != nullptr);
+  EXPECT_EQ(physics->GetType(), "ode");
+  {
+    std::string solver;
+    EXPECT_NO_THROW(
+      solver = boost::any_cast<std::string>(physics->GetParam("solver_type")));
+    EXPECT_EQ("world", solver);
+  }
+  {
+    bool odeQuiet = false;
+    EXPECT_NO_THROW(
+      odeQuiet = boost::any_cast<bool>(physics->GetParam("ode_quiet")));
+    EXPECT_TRUE(odeQuiet);
+  }
 }

test/worlds/world_step.world

@@ -3,6 +3,7 @@
     <physics type="ode">
       <gravity>0.000000 0.000000 -9.810000</gravity>
       <ode>
+        <ode_quiet>1</ode_quiet>
         <solver>
           <type>world</type>
           <iters>250</iters>