Merge branch 'sdf12' into aditya/inertial_stats

include/sdf/Camera.hh

@@ -152,6 +152,14 @@ namespace sdf
     /// \param[in] _fmt The pixel format string.
     public: void SetPixelFormatStr(const std::string &_fmt);
 
+    /// \brief Get the anti-aliasing value.
+    /// \return The anti-aliasing value.
+    public: uint32_t AntiAliasingValue() const;
+
+    /// \brief Set the anti-aliasing value.
+    /// \param[in] _antiAliasingValue The anti-aliasing value.
+    public: void SetAntiAliasingValue(uint32_t _antiAliasingValue);
+
     /// \brief Get the near clip distance for the depth camera.
     /// \return The near clip depth distance.
     public: double DepthNearClip() const;

include/sdf/Plugin.hh

@@ -58,6 +58,16 @@ namespace sdf
     /// \param[in] _plugin Plugin to copy.
     public: Plugin(Plugin &&_plugin) noexcept;
 
+    /// \brief A constructor that initializes the plugin's filename, name, and
+    /// optionally the content.
+    /// \param[in] _filename Filename of the shared library associated with
+    /// this plugin.
+    /// \param[in] _name The name of the plugin.
+    /// \param[in] _xmlContent Optional XML content that will be stored in
+    /// this plugin.
+    public: Plugin(const std::string &_filename, const std::string &_name,
+                   const std::string &_xmlContent = "");
+
     /// \brief Load the plugin based on a element pointer. This is *not* the
     /// usual entry point. Typical usage of the SDF DOM is through the Root
     /// object.
@@ -97,6 +107,15 @@ namespace sdf
     /// \param[in] _elem Element to insert.
     public: void InsertContent(const sdf::ElementPtr _elem);
 
+    /// \brief Insert XML content into this plugin. This function does not
+    /// modify the values in the sdf::ElementPtr returned by the `Element()`
+    /// function. The provided content must be valid XML.
+    /// \param[in] _content A string that contains valid XML. The XML is
+    /// inserted into this plugin if it is valid.
+    /// \return False if the provided content was invalid, in which case the
+    /// content of this plugin is not modified. True otherwise
+    public: bool InsertContent(const std::string _content);
+
     /// \brief Set the filename of the shared library.
     /// \param[in] _filename Filename of the shared library associated with
     /// this plugin.

sdf/1.7/camera.sdf

@@ -20,6 +20,9 @@
     <element name="format" type="string" default="R8G8B8" required="0">
       <description>(L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)</description>
     </element>
+    <element name="anti_aliasing" type="int" default="4" required="0">
+      <description>Value used for anti-aliasing</description>
+    </element>
   </element> <!-- End Image -->
 
   <element name="clip" required="1">

sdf/1.8/camera.sdf

@@ -20,6 +20,9 @@
     <element name="format" type="string" default="R8G8B8" required="0">
       <description>(L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)</description>
     </element>
+    <element name="anti_aliasing" type="int" default="4" required="0">
+      <description>Value used for anti-aliasing</description>
+    </element>
   </element> <!-- End Image -->
 
   <element name="clip" required="1">

sdf/1.9/camera.sdf

@@ -28,6 +28,9 @@
     <element name="format" type="string" default="R8G8B8" required="0">
       <description>(L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)</description>
     </element>
+    <element name="anti_aliasing" type="int" default="4" required="0">
+      <description>Value used for anti-aliasing</description>
+    </element>
   </element> <!-- End Image -->
 
   <element name="clip" required="1">

sdf/1.9/model.sdf

@@ -14,17 +14,25 @@
   <attribute name="canonical_link" type="string" default="" required="0">
     <description>
       The name of the model's canonical link, to which the model's implicit
-      coordinate frame is attached. If unset or set to an empty string,
-      the first link element listed as a child of this model is chosen
-      as the canonical link.
+      coordinate frame is attached. If unset or set to an empty string, the
+      first `/link` listed as a direct child of this model is chosen as the
+      canonical link. If the model has no direct `/link` children, it will
+      instead be attached to the first nested (or included) model's implicit
+      frame.
     </description>
   </attribute>
   <attribute name="placement_frame" type="string" default="" required="0">
     <description>The frame inside this model whose pose will be set by the pose element of the model. i.e, the pose element specifies the pose of this frame instead of the model frame.</description>
   </attribute>
 
   <element name="static" type="bool" default="false" required="0">
-    <description>If set to true, the model is immovable. Otherwise the model is simulated in the dynamics engine.</description>
+    <description>
+      If set to true, the model is immovable; i.e., a dynamics engine will not
+      update its position. This will also overwrite this model's `@canonical_link`
+      and instead attach the model's implicit frame to the world's implicit frame.
+      This holds even if this model is nested (or included) by another model
+      instead of being a direct child of `//world`.
+    </description>
   </element>
 
   <element name="self_collide" type="bool" default="false" required="0">

src/CMakeLists.txt

@@ -69,6 +69,7 @@ if (BUILD_TESTING)
 
   if (TARGET UNIT_FrameSemantics_TEST)
     target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc)
+    target_link_libraries(UNIT_FrameSemantics_TEST TINYXML2::TINYXML2)
   endif()
 
   if (TARGET UNIT_ParamPassing_TEST)
@@ -89,6 +90,7 @@ if (BUILD_TESTING)
 
   if (TARGET UNIT_Utils_TEST)
     target_sources(UNIT_Utils_TEST PRIVATE Utils.cc)
+    target_link_libraries(UNIT_Utils_TEST TINYXML2::TINYXML2)
   endif()
 
   if (TARGET UNIT_XmlUtils_TEST)

src/Camera.cc

@@ -73,6 +73,9 @@ class sdf::Camera::Implementation
   /// \brief Image format.
   public: PixelFormatType pixelFormat{PixelFormatType::RGB_INT8};
 
+  /// \brief Anti-aliasing value.
+  public: uint32_t antiAliasingValue{4};
+
   /// \brief Near clip distance.
   public: double nearClip{0.1};
 
@@ -266,6 +269,10 @@ Errors Camera::Load(ElementPtr _sdf)
       errors.push_back({ErrorCode::ELEMENT_INVALID,
         "Camera sensor <image><format> has invalid value of " + format});
     }
+
+    this->dataPtr->antiAliasingValue =
+        elem->Get<uint32_t>("anti_aliasing",
+            this->dataPtr->antiAliasingValue).first;
   }
   else
   {
@@ -500,6 +507,18 @@ void Camera::SetPixelFormatStr(const std::string &_fmt)
   this->dataPtr->pixelFormat = ConvertPixelFormat(_fmt);
 }
 
+//////////////////////////////////////////////////
+uint32_t Camera::AntiAliasingValue() const
+{
+  return this->dataPtr->antiAliasingValue;
+}
+
+//////////////////////////////////////////////////
+void Camera::SetAntiAliasingValue(uint32_t _antiAliasingValue)
+{
+  this->dataPtr->antiAliasingValue = _antiAliasingValue;
+}
+
 //////////////////////////////////////////////////
 double Camera::DepthNearClip() const
 {
@@ -1043,6 +1062,9 @@ sdf::ElementPtr Camera::ToElement() const
   imageElem->GetElement("width")->Set<double>(this->ImageWidth());
   imageElem->GetElement("height")->Set<double>(this->ImageHeight());
   imageElem->GetElement("format")->Set<std::string>(this->PixelFormatStr());
+  imageElem->GetElement("anti_aliasing")->Set<uint32_t>(
+      this->AntiAliasingValue());
+
   sdf::ElementPtr clipElem = elem->GetElement("clip");
   clipElem->GetElement("near")->Set<double>(this->NearClip());
   clipElem->GetElement("far")->Set<double>(this->FarClip());

src/Camera_TEST.cc

@@ -50,6 +50,10 @@ TEST(DOMCamera, Construction)
   cam.SetPixelFormat(sdf::PixelFormatType::L_INT8);
   EXPECT_EQ(sdf::PixelFormatType::L_INT8 , cam.PixelFormat());
 
+  EXPECT_EQ(4u, cam.AntiAliasingValue());
+  cam.SetAntiAliasingValue(8);
+  EXPECT_EQ(8u, cam.AntiAliasingValue());
+
   EXPECT_DOUBLE_EQ(0.1, cam.DepthNearClip());
   EXPECT_FALSE(cam.HasDepthNearClip());
   cam.SetDepthNearClip(0.2);

src/Plugin.cc

@@ -15,6 +15,7 @@
  *
 */
 
+#include "sdf/Types.hh"
 #include "sdf/Plugin.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
@@ -42,6 +43,18 @@ Plugin::Plugin()
 {
 }
 
+/////////////////////////////////////////////////
+Plugin::Plugin(const std::string &_filename, const std::string &_name,
+               const std::string &_xmlContent)
+  : dataPtr(std::make_unique<sdf::PluginPrivate>())
+{
+  this->SetFilename(_filename);
+  this->SetName(_name);
+  std::string trimmed = sdf::trim(_xmlContent);
+  if (!trimmed.empty())
+    this->InsertContent(trimmed);
+}
+
 /////////////////////////////////////////////////
 Plugin::~Plugin() = default;
 
@@ -175,6 +188,50 @@ void Plugin::InsertContent(const sdf::ElementPtr _elem)
   this->dataPtr->contents.push_back(_elem->Clone());
 }
 
+/////////////////////////////////////////////////
+bool Plugin::InsertContent(const std::string _content)
+{
+  // Read the XML content
+  auto xmlDoc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE);;
+  xmlDoc.Parse(_content.c_str());
+  if (xmlDoc.Error())
+  {
+    sdferr << "Error parsing XML from string: " << xmlDoc.ErrorStr() << '\n';
+    return false;
+  }
+
+  // Insert each XML element
+  for (tinyxml2::XMLElement *xml = xmlDoc.FirstChildElement(); xml;
+       xml = xml->NextSiblingElement())
+  {
+    sdf::ElementPtr element(new sdf::Element);
+
+    // Copy the name
+    element->SetName(xml->Name());
+
+    // Copy attributes
+    for (const tinyxml2::XMLAttribute *attribute = xml->FirstAttribute();
+        attribute; attribute = attribute->Next())
+    {
+      element->AddAttribute(attribute->Name(), "string", "", 1, "");
+      element->GetAttribute(attribute->Name())->SetFromString(
+          attribute->Value());
+    }
+
+    // Copy the value
+    if (xml->GetText() != nullptr)
+      element->AddValue("string", xml->GetText(), true);
+
+    // Copy all children
+    copyChildren(element, xml, false);
+
+    // Add the element to this plugin
+    this->InsertContent(element);
+  }
+
+  return true;
+}
+
 /////////////////////////////////////////////////
 Plugin &Plugin::operator=(const Plugin &_plugin)
 {

src/Plugin_TEST.cc

@@ -318,3 +318,70 @@ TEST(DOMPlugin, InputStreamOperator)
   std::string elemString = elem->ToString("");
   EXPECT_EQ(input, elemString);
 }
+
+/////////////////////////////////////////////////
+TEST(DOMPlugin, InsertStringContent)
+{
+  sdf::Plugin plugin("my-filename", "my-name",
+      "<render_engine>ogre2</render_engine>");
+  EXPECT_EQ("my-filename", plugin.Filename());
+  EXPECT_EQ("my-name", plugin.Name());
+  ASSERT_EQ(1u, plugin.Contents().size());
+  EXPECT_EQ("render_engine", plugin.Contents()[0]->GetName());
+  EXPECT_EQ("ogre2", plugin.Contents()[0]->Get<std::string>());
+
+  std::string extraContent = R"foo(
+  <with_attribute value='bar'>1.234</with_attribute>
+  <sibling>hello</sibling>
+  <with_children>
+    <child1>goodbye</child1>
+    <child2>goodbye</child2>
+  </with_children>
+)foo";
+
+  // Insert more content using a string
+  EXPECT_TRUE(plugin.InsertContent(extraContent));
+
+  std::ostringstream completeContent;
+  completeContent << "  <render_engine>ogre2</render_engine>" << extraContent;
+
+  std::ostringstream completePlugin;
+  completePlugin << "<plugin name='my-name' filename='my-filename'>\n"
+    << completeContent.str()
+    << "</plugin>\n";
+  EXPECT_EQ(completePlugin.str(), plugin.ToElement()->ToString(""));
+
+  // Try out curly braces intitialization
+  sdf::Plugin plugin2{plugin.Filename(), plugin.Name(), completeContent.str()};
+  EXPECT_EQ(plugin.ToElement()->ToString(""),
+            plugin2.ToElement()->ToString(""));
+
+  // Try to insert poorly formed XML, which should fail and not modify the
+  // content.
+  EXPECT_FALSE(plugin2.InsertContent("<a></b>"));
+  EXPECT_EQ(plugin.ToElement()->ToString(""),
+            plugin2.ToElement()->ToString(""));
+
+  // An empty string will also fail and not modify the content
+  EXPECT_FALSE(plugin2.InsertContent(""));
+  EXPECT_EQ(plugin.ToElement()->ToString(""),
+            plugin2.ToElement()->ToString(""));
+
+  // Contructing a new plugin with no content
+  sdf::Plugin plugin3{"a filename", "a name"};
+  EXPECT_EQ("a filename", plugin3.Filename());
+  EXPECT_EQ("a name", plugin3.Name());
+  EXPECT_TRUE(plugin3.Contents().empty());
+
+  // Contructing a new plugin with bad XML content
+  sdf::Plugin plugin4{"filename", "name", "<garbage>"};
+  EXPECT_EQ("filename", plugin4.Filename());
+  EXPECT_EQ("name", plugin4.Name());
+  EXPECT_TRUE(plugin4.Contents().empty());
+
+  // Contructing a new plugin with bad XML content
+  sdf::Plugin plugin5{"filename", "name", "    "};
+  EXPECT_EQ("filename", plugin5.Filename());
+  EXPECT_EQ("name", plugin5.Name());
+  EXPECT_TRUE(plugin5.Contents().empty());
+}

src/Utils.cc

@@ -304,5 +304,63 @@ sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf,
 
   return allErrors;
 }
+
+/////////////////////////////////////////////////
+void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml,
+    const bool _onlyUnknown)
+{
+  // Iterate over all the child elements
+  tinyxml2::XMLElement *elemXml = nullptr;
+  for (elemXml = _xml->FirstChildElement(); elemXml;
+       elemXml = elemXml->NextSiblingElement())
+  {
+    std::string elemName = elemXml->Name();
+
+    if (_sdf->HasElementDescription(elemName))
+    {
+      if (!_onlyUnknown)
+      {
+        sdf::ElementPtr element = _sdf->AddElement(elemName);
+
+        // FIXME: copy attributes
+        for (const auto *attribute = elemXml->FirstAttribute();
+             attribute; attribute = attribute->Next())
+        {
+          element->GetAttribute(attribute->Name())->SetFromString(
+            attribute->Value());
+        }
+
+        // copy value
+        const char *value = elemXml->GetText();
+        if (value)
+        {
+          element->GetValue()->SetFromString(value);
+        }
+        copyChildren(element, elemXml, _onlyUnknown);
+      }
+    }
+    else
+    {
+      sdf::ElementPtr element(new sdf::Element);
+      element->SetParent(_sdf);
+      element->SetName(elemName);
+      for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute();
+           attribute; attribute = attribute->Next())
+      {
+        element->AddAttribute(attribute->Name(), "string", "", 1, "");
+        element->GetAttribute(attribute->Name())->SetFromString(
+            attribute->Value());
+      }
+
+      if (elemXml->GetText() != nullptr)
+      {
+        element->AddValue("string", elemXml->GetText(), true);
+      }
+
+      copyChildren(element, elemXml, _onlyUnknown);
+      _sdf->InsertElement(element);
+    }
+  }
+}
 }
 }