Merge 1d755ae into b7f0cba

include/sdf/parser.hh

@@ -115,6 +115,21 @@ namespace sdf
   SDFORMAT_VISIBLE
   bool readFile(const std::string &_filename, SDFPtr _sdf);
 
+  /// \brief Populate the SDF values from a file
+  ///
+  /// This populates the given sdf pointer from a file. If the file is a URDF
+  /// file it is converted to SDF first. All files are converted to the latest
+  /// SDF version
+  /// \param[in] _filename Name of the SDF file
+  /// \param[in] _sdf Pointer to an SDF object.
+  /// \param[in] _enable_new_warnings Consider previous issues in debug log as
+  ///            warnings. This allow to keep log level backwards compatible
+  ///            while providing an option to increase the warnings with new
+  ///            relevant issues.
+  /// \return True if successful.
+  bool readFile(const std::string &_filename, SDFPtr _sdf,
+      bool _enable_new_warnings);
+
   /// \brief Populate the SDF values from a string
   ///
   /// This populates the sdf pointer from a string. If the string is a URDF

include/sdf/parser_urdf.hh

@@ -42,6 +42,15 @@ namespace sdf
     /// \brief constructor
     public: URDF2SDF();
 
+    /// \brief constructor
+    /// \param[in] _enable_new_warnings Consider previous issues in debug log as
+    ///            warnings.
+    public: explicit URDF2SDF(const bool _enable_new_warnings) :
+            URDF2SDF()
+      {
+        enable_new_warnings = _enable_new_warnings;
+      };
+
     /// \brief destructor
     public: ~URDF2SDF();
 
@@ -81,6 +90,9 @@ namespace sdf
     // cppcheck-suppress unusedPrivateFunction
     // cppcheck-suppress unmatchedSuppression
     private: void ListSDFExtensions(const std::string &_reference);
+
+    /// when to raise new warnings from previous log issues
+    private: bool enable_new_warnings;
   };
   }
 }

src/parser.cc

@@ -54,12 +54,15 @@ inline namespace SDF_VERSION_NAMESPACE {
 /// \param[in] _sdf Pointer to an SDF object.
 /// \param[in] _convert Convert to the latest version if true.
 /// \param[out] _errors Parsing errors will be appended to this variable.
+/// \param[in] _enable_new_warnings Consider previous issues in debug log as
+///            warnings.
 /// \return True if successful.
 bool readFileInternal(
     const std::string &_filename,
     SDFPtr _sdf,
     const bool _convert,
-    Errors &_errors);
+    Errors &_errors,
+    bool _enable_new_warnings);
 
 /// \brief Internal helper for readString, which populates the SDF values
 /// from a string
@@ -71,12 +74,15 @@ bool readFileInternal(
 /// \param[in] _sdf Pointer to an SDF object.
 /// \param[in] _convert Convert to the latest version if true.
 /// \param[out] _errors Parsing errors will be appended to this variable.
+/// \param[in] _enable_new_warnings Consider previous issues in debug log as
+///            warnings.
 /// \return True if successful.
 bool readStringInternal(
     const std::string &_xmlString,
     SDFPtr _sdf,
     const bool _convert,
-    Errors &_errors);
+    Errors &_errors,
+    bool _enable_new_warnings);
 
 //////////////////////////////////////////////////
 template <typename TPtr>
@@ -364,19 +370,26 @@ bool readFile(const std::string &_filename, SDFPtr _sdf)
 //////////////////////////////////////////////////
 bool readFile(const std::string &_filename, SDFPtr _sdf, Errors &_errors)
 {
-  return readFileInternal(_filename, _sdf, true, _errors);
+  return readFileInternal(_filename, _sdf, true, _errors, false);
+}
+
+//////////////////////////////////////////////////
+bool readFile(const std::string &_filename, SDFPtr _sdf, Errors &_errors,
+    const bool _enable_new_warnings)
+{
+  return readFileInternal(_filename, _sdf, true, _errors, _enable_new_warnings);
 }
 
 //////////////////////////////////////////////////
 bool readFileWithoutConversion(
     const std::string &_filename, SDFPtr _sdf, Errors &_errors)
 {
-  return readFileInternal(_filename, _sdf, false, _errors);
+  return readFileInternal(_filename, _sdf, false, _errors, false);
 }
 
 //////////////////////////////////////////////////
 bool readFileInternal(const std::string &_filename, SDFPtr _sdf,
-      const bool _convert, Errors &_errors)
+      const bool _convert, Errors &_errors, bool _enable_new_warnings)
 {
   TiXmlDocument xmlDoc;
   std::string filename = sdf::findFile(_filename, true, true);
@@ -413,7 +426,7 @@ bool readFileInternal(const std::string &_filename, SDFPtr _sdf,
   }
   else if (URDF2SDF::IsURDF(filename))
   {
-    URDF2SDF u2g;
+    URDF2SDF u2g(_enable_new_warnings);
     TiXmlDocument doc = u2g.InitModelFile(filename);
     if (sdf::readDoc(&doc, _sdf, "urdf file", _convert, _errors))
     {
@@ -447,19 +460,19 @@ bool readString(const std::string &_xmlString, SDFPtr _sdf)
 //////////////////////////////////////////////////
 bool readString(const std::string &_xmlString, SDFPtr _sdf, Errors &_errors)
 {
-  return readStringInternal(_xmlString, _sdf, true, _errors);
+  return readStringInternal(_xmlString, _sdf, true, _errors, false);
 }
 
 //////////////////////////////////////////////////
 bool readStringWithoutConversion(
     const std::string &_filename, SDFPtr _sdf, Errors &_errors)
 {
-  return readStringInternal(_filename, _sdf, false, _errors);
+  return readStringInternal(_filename, _sdf, false, _errors, false);
 }
 
 //////////////////////////////////////////////////
 bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf,
-    const bool _convert, Errors &_errors)
+    const bool _convert, Errors &_errors, bool _enable_new_warnings)
 {
   TiXmlDocument xmlDoc;
   xmlDoc.Parse(_xmlString.c_str());
@@ -475,9 +488,9 @@ bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf,
   else
   {
     SDF_SUPPRESS_DEPRECATED_BEGIN
-    URDF2SDF u2g;
+    URDF2SDF u2g(_enable_new_warnings);
     SDF_SUPPRESS_DEPRECATED_END
-    TiXmlDocument doc = u2g.InitModelString(_xmlString);
+    TiXmlDocument doc = u2g.InitModelString(_xmlString, true);
     if (sdf::readDoc(&doc, _sdf, "urdf string", _convert, _errors))
     {
       sdfdbg << "Parsing from urdf.\n";

src/parser_urdf.cc

@@ -142,7 +142,8 @@ void AddTransform(TiXmlElement *_elem,
     const ignition::math::Pose3d &_transform);
 
 /// create SDF from URDF link
-void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link);
+void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link,
+    bool _enable_new_warnings);
 
 /// create SDF Link block based on URDF
 void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link,
@@ -1066,15 +1067,15 @@ URDF2SDF::URDF2SDF()
   g_initialRobotPoseValid = false;
   g_fixedJointsTransformedInRevoluteJoints.clear();
   g_fixedJointsTransformedInFixedJoints.clear();
+
+  enable_new_warnings = false;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 URDF2SDF::~URDF2SDF()
 {
 }
 
-
-
 ////////////////////////////////////////////////////////////////////////////////
 std::string Values2str(unsigned int _count, const double *_values)
 {
@@ -2601,42 +2602,62 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference)
   }
 }
 
+////////////////////////////////////////////////////////////////////////////////
+void _DisplayDbgOrWarning(const std::string & _msg,
+    const bool _enable_new_warnings)
+{
+  if (_enable_new_warnings)
+    sdfwarn << _msg;
+  else
+    sdfdbg << _msg;
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 void CreateSDF(TiXmlElement *_root,
-               urdf::LinkConstSharedPtr _link)
+               urdf::LinkConstSharedPtr _link,
+               bool _enable_new_warnings)
 {
   // must have an <inertial> block and cannot have zero mass.
   //  allow det(I) == zero, in the case of point mass geoms.
   // @todo:  keyword "world" should be a constant defined somewhere else
   if (_link->name != "world" &&
       ((!_link->inertial) || ignition::math::equal(_link->inertial->mass, 0.0)))
   {
+    if (_link->inertial && ignition::math::equal(_link->inertial->mass, 0.0))
+    {
+      _DisplayDbgOrWarning(std::string("urdf2sdf: link[" + _link->name)
+             + "] has a mass considered zero ["
+             + std::to_string(_link->inertial->mass) +
+             + "].It is ignored.\n", _enable_new_warnings);
+      return;
+    }
+
     if (!_link->child_links.empty())
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, ["
-             << static_cast<int>(_link->child_links.size())
-             << "] children links ignored.\n";
+      _DisplayDbgOrWarning(std::string("urdf2sdf: link[" + _link->name)
+             + "] has no inertia, ["
+             + std::to_string(_link->child_links.size())
+             + "] children links ignored.\n", _enable_new_warnings);
     }
 
     if (!_link->child_joints.empty())
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, ["
-             << static_cast<int>(_link->child_links.size())
-             << "] children joints ignored.\n";
+      _DisplayDbgOrWarning(std::string("urdf2sdf: link[" + _link->name)
+             + "] has no inertia, ["
+             + std::to_string(_link->child_links.size())
+             + "] children joints ignored.\n", _enable_new_warnings);
     }
 
     if (_link->parent_joint)
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, "
-             << "parent joint [" << _link->parent_joint->name
-             << "] ignored.\n";
+      _DisplayDbgOrWarning(std::string("urdf2sdf: link[" + _link->name)
+             + "] has no inertia, "
+             + "parent joint [" + _link->parent_joint->name
+             + "] ignored.\n", _enable_new_warnings);
     }
 
-    sdfdbg << "urdf2sdf: link[" << _link->name
-           << "] has no inertia, not modeled in sdf\n";
+    _DisplayDbgOrWarning(std::string("urdf2sdf: link[" + _link->name)
+           + "] has no inertia, not modeled in sdf\n", _enable_new_warnings);
     return;
   }
 
@@ -2652,7 +2673,7 @@ void CreateSDF(TiXmlElement *_root,
   // recurse into children
   for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
   {
-    CreateSDF(_root, _link->child_links[i]);
+    CreateSDF(_root, _link->child_links[i], _enable_new_warnings);
   }
 }
 
@@ -3186,13 +3207,13 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
           child = rootLink->child_links.begin();
           child != rootLink->child_links.end(); ++child)
       {
-        CreateSDF(robot, (*child));
+        CreateSDF(robot, (*child), enable_new_warnings);
       }
     }
     else
     {
       // convert, starting from root link
-      CreateSDF(robot, rootLink);
+      CreateSDF(robot, rootLink, enable_new_warnings);
     }
 
     // insert the extensions without reference into <robot> root level
@@ -3234,7 +3255,7 @@ TiXmlDocument URDF2SDF::InitModelDoc(TiXmlDocument* _xmlDoc)
   std::ostringstream stream;
   stream << *_xmlDoc;
   std::string urdfStr = stream.str();
-  return InitModelString(urdfStr);
+  return InitModelString(urdfStr, true);
 }
 
 ////////////////////////////////////////////////////////////////////////////////

src/parser_urdf_TEST.cc

@@ -22,6 +22,8 @@
 #include "sdf/sdf.hh"
 #include "sdf/parser_urdf.hh"
 
+#include "test_utils.hh"
+
 /////////////////////////////////////////////////
 std::string getMinimalUrdfTxt()
 {
@@ -332,6 +334,77 @@ TEST(URDFParser, ParseGazeboLinkFactors)
   }
 }
 
+/////////////////////////////////////////////////
+TEST(URDFParser, EnableNewWarnings)
+{
+    // Redirect sdfwarn output (copied from model_dom.cc)
+    std::stringstream buffer;
+    sdf::testing::RedirectConsoleStream redir(
+        sdf::Console::Instance()->GetMsgStream(), &buffer);
+#ifdef _WIN32
+    sdf::Console::Instance()->SetQuiet(false);
+    sdf::testing::ScopeExit revertSetQuiet(
+        []
+        {
+          sdf::Console::Instance()->SetQuiet(true);
+        });
+#endif
+  SDF_SUPPRESS_DEPRECATED_BEGIN
+  sdf::URDF2SDF parser_(true);
+  SDF_SUPPRESS_DEPRECATED_END
+
+  {
+    TiXmlDocument doc;
+    buffer.str("");
+    std::ostringstream stream_no_inertial;
+    // Testing no inertia and inertia 0.0
+    stream_no_inertial << "<robot name=\"test\">"
+                       << "  <link name=\"link_no_inertia\">"
+                       << "    <visual>"
+                       << "      <geometry>"
+                       << "         <sphere radius=\"1.0\"/>"
+                       << "      </geometry>"
+                       << "    </visual>"
+                       << "  </link>"
+                       << "</robot>";
+    doc.Parse(stream_no_inertial.str().c_str());
+    ASSERT_NO_THROW(parser_.InitModelDoc(&doc));
+    // Check warning message
+    EXPECT_NE(
+      std::string::npos, buffer.str().find("] has no inertia"))
+      << buffer.str();
+  }
+
+  {
+    TiXmlDocument doc;
+    buffer.str("");
+    std::ostringstream stream_no_mass;
+    // Testing mass zero
+    stream_no_mass << "<robot name=\"test\">"
+                   << "  <link name=\"link_no_mass\">"
+                   << "    <visual>"
+                   << "      <geometry>"
+                   << "         <sphere radius=\"1.0\"/>"
+                   << "      </geometry>"
+                   << "    </visual>"
+                   << "    <inertial>"
+                   << "      <mass value=\"0.000001\" />"
+                   << "      <origin xyz=\"0 0 0\" />"
+                   << "      <inertia ixx=\"0.001\" ixy=\"0.0\" ixz=\"0.0\""
+                   << "               iyy=\"0.001\" iyz=\"0.0\""
+                   << "               izz=\"0.001\" />"
+                   << "    </inertial>"
+                   << "  </link>"
+                   << "</robot>";
+  doc.Parse(stream_no_mass.str().c_str());
+  ASSERT_NO_THROW(parser_.InitModelDoc(&doc));
+  // Check warning message
+  EXPECT_NE(
+    std::string::npos, buffer.str().find("] has a mass considered zero ["))
+    << buffer.str();
+  }
+}
+
 /////////////////////////////////////////////////
 TEST(URDFParser, ParseGazeboInvalidDampingFactor)
 {