Fixed application of <sensor><pose> tags in lumped linkes during URDF…

… conversion.

Fixed <projector>, too.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>

src/parser_urdf.cc

@@ -2125,6 +2125,8 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
         for (auto blobIt = (*ge)->blobs.begin();
             blobIt != (*ge)->blobs.end(); ++blobIt)
         {
+          // Be sure to always copy only the first element; code in
+          // ReduceSDFExtensionSensorTransformReduction depends in this behavior
           CopyBlob((*blobIt)->FirstChildElement(), _elem);
         }
       }
@@ -3354,8 +3356,8 @@ void ReduceSDFExtensionSensorTransformReduction(
     std::vector<XMLDocumentPtr>::iterator _blobIt,
     ignition::math::Pose3d _reductionTransform)
 {
-  // overwrite <xyz> and <rpy> if they exist
-  if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0)
+  auto sensorElement = (*_blobIt)->FirstChildElement();
+  if ( strcmp(sensorElement->Name(), "sensor") == 0)
   {
     // parse it and add/replace the reduction transform
     // find first instance of xyz and rpy, replace with reduction transform
@@ -3369,16 +3371,58 @@ void ReduceSDFExtensionSensorTransformReduction(
     //   sdfdbg << "    " << streamIn.CStr() << "\n";
     // }
 
+    auto sensorPose {ignition::math::Pose3d::Zero};
     {
-      tinyxml2::XMLNode *oldPoseKey = (*_blobIt)->FirstChildElement("pose");
-      /// @todo: FIXME:  we should read xyz, rpy and aggregate it to
-      /// reductionTransform instead of just throwing the info away.
+      auto sensorPosText = "0 0 0 0 0 0";
+      const auto& oldPoseKey = sensorElement->FirstChildElement("pose");
       if (oldPoseKey)
       {
-        (*_blobIt)->DeleteChild(oldPoseKey);
+        const auto& poseElemXml = oldPoseKey->ToElement();
+        if (poseElemXml->Attribute("relative_to"))
+          return;
+
+        // see below for explanation; if a sibling element exists, it stores the
+        // original <sensor><pose> tag content
+        const auto& sibling = sensorElement->NextSibling();
+        if (poseElemXml->GetText() && !sibling)
+          sensorPosText = poseElemXml->GetText();
+        else if (sibling && sibling->ToElement()->GetText())
+          sensorPosText = sibling->ToElement()->GetText();
+        else
+        {
+          sdferr << "Unexpected case in sensor pose computation\n";
+          return;
+        }
+
+        // delete the <pose> tag, we'll add a new one at the end
+        sensorElement->DeleteChild(oldPoseKey);
+      }
+
+      // parse the 6-tuple text into math::Pose3d
+      std::stringstream  ss;
+      ss.imbue(std::locale::classic());
+      ss << sensorPosText;
+      ss >> sensorPose;
+      if (ss.fail())
+      {
+        sdferr << "Could not parse <sensor><pose>: [" << sensorPosText << "]\n";
+        return;
       }
+
+      // critical part: we store the original <pose> tag from <sensor> actually
+      // as a sibling of <sensor>... only first element of the blob is processed
+      // further, so its siblings can be used as temporary storage; we store the
+      // original <pose> tag there so that we can use the <sensor><pose> tag
+      // for storing the reduced position
+      auto doc = (*_blobIt)->GetDocument();
+      const auto& poseTxt = doc->NewText(sensorPosText);
+      auto poseKey = doc->NewElement("pose");
+      poseKey->LinkEndChild(poseTxt);
+      (*_blobIt)->LinkEndChild(poseKey);
     }
 
+    _reductionTransform = _reductionTransform * sensorPose;
+
     // convert reductionTransform to values
     urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
                                _reductionTransform.Pos().Y(),
@@ -3403,7 +3447,7 @@ void ReduceSDFExtensionSensorTransformReduction(
 
     poseKey->LinkEndChild(poseTxt);
 
-    (*_blobIt)->LinkEndChild(poseKey);
+    sensorElement->LinkEndChild(poseKey);
   }
 }
 
@@ -3412,8 +3456,8 @@ void ReduceSDFExtensionProjectorTransformReduction(
     std::vector<XMLDocumentPtr>::iterator _blobIt,
     ignition::math::Pose3d _reductionTransform)
 {
-  // overwrite <pose> (xyz/rpy) if it exists
-  if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "projector") == 0)
+  auto projectorElement = (*_blobIt)->FirstChildElement();
+  if ( strcmp(projectorElement->Name(), "projector") == 0)
   {
     // parse it and add/replace the reduction transform
     // find first instance of xyz and rpy, replace with reduction transform
@@ -3426,16 +3470,59 @@ void ReduceSDFExtensionProjectorTransformReduction(
     //   sdfdbg << "    " << streamIn << "\n";
     // }
 
-    // should read <pose>...</pose> and agregate reductionTransform
-    tinyxml2::XMLNode *poseKey = (*_blobIt)->FirstChildElement("pose");
-    // read pose and save it
-
-    // remove the tag for now
-    if (poseKey)
+    auto projectorPose {ignition::math::Pose3d::Zero};
     {
-      (*_blobIt)->DeleteChild(poseKey);
+      auto projectorPosText = "0 0 0 0 0 0";
+      const auto& oldPoseKey = projectorElement->FirstChildElement("pose");
+      if (oldPoseKey)
+      {
+        const auto& poseElemXml = oldPoseKey->ToElement();
+        if (poseElemXml->Attribute("relative_to"))
+          return;
+
+        // see below for explanation; if a sibling element exists, it stores the
+        // original <projector><pose> tag content
+        const auto& sibling = projectorElement->NextSibling();
+        if (poseElemXml->GetText() && !sibling)
+          projectorPosText = poseElemXml->GetText();
+        else if (sibling && sibling->ToElement()->GetText())
+          projectorPosText = sibling->ToElement()->GetText();
+        else
+        {
+          sdferr << "Unexpected case in projector pose computation\n";
+          return;
+        }
+
+        // delete the <pose> tag, we'll add a new one at the end
+        projectorElement->DeleteChild(oldPoseKey);
+      }
+
+      // parse the 6-tuple text into math::Pose3d
+      std::stringstream  ss;
+      ss.imbue(std::locale::classic());
+      ss << projectorPosText;
+      ss >> projectorPose;
+      if (ss.fail())
+      {
+        sdferr << "Could not parse <projector><pose>: ["
+               << projectorPosText << "]\n";
+        return;
+      }
+
+      // critical part: we store the original <pose> tag from <projector>
+      // actually as a sibling of <projector>... only first element of the blob
+      // is processed further, so its siblings can be used as temporary storage;
+      // we store the original <pose> tag there so that we can use the
+      // <projector><pose> tag for storing the reduced position
+      auto doc = (*_blobIt)->GetDocument();
+      const auto& poseTxt = doc->NewText(projectorPosText);
+      auto poseKey = doc->NewElement("pose");
+      poseKey->LinkEndChild(poseTxt);
+      (*_blobIt)->LinkEndChild(poseKey);
     }
 
+    _reductionTransform = _reductionTransform * projectorPose;
+
     // convert reductionTransform to values
     urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
                                _reductionTransform.Pos().Y(),
@@ -3456,10 +3543,10 @@ void ReduceSDFExtensionProjectorTransformReduction(
 
     auto* doc = (*_blobIt)->GetDocument();
     tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str());
-    poseKey = doc->NewElement("pose");
+    auto poseKey = doc->NewElement("pose");
     poseKey->LinkEndChild(poseTxt);
 
-    (*_blobIt)->LinkEndChild(poseKey);
+    projectorElement->LinkEndChild(poseKey);
   }
 }
 

test/integration/urdf_gazebo_extensions.cc

@@ -99,4 +99,163 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
       EXPECT_TRUE(physics->Get<bool>("implicit_spring_damper"));
     }
   }
+
+  sdf::ElementPtr link0;
+  for (sdf::ElementPtr link = model->GetElement("link"); link;
+       link = link->GetNextElement("link"))
+  {
+    const auto& linkName = link->Get<std::string>("name");
+    if (linkName == "link0")
+    {
+      link0 = link;
+      break;
+    }
+  }
+  ASSERT_TRUE(link0);
+
+  bool foundSensorNoPose {false};
+  bool foundSensorPose {false};
+  bool foundSensorPoseRelative {false};
+  bool foundSensorPoseTwoLevel {false};
+  bool foundIssue378Sensor {false};
+  bool foundIssue67Sensor {false};
+
+  for (sdf::ElementPtr sensor = link0->GetElement("sensor"); sensor;
+       sensor = sensor->GetNextElement("sensor"))
+  {
+    const auto& sensorName = sensor->Get<std::string>("name");
+    if (sensorName == "sensorNoPose")
+    {
+      foundSensorNoPose = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_FLOAT_EQ(pose.X(), 333.0);
+      EXPECT_FLOAT_EQ(pose.Y(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Z(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Roll(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Pitch(), 0.0);
+      EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+    else if (sensorName == "sensorPose")
+    {
+      foundSensorPose = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_FLOAT_EQ(pose.X(), 333.0);
+      EXPECT_FLOAT_EQ(pose.Y(), 111.0);
+      EXPECT_FLOAT_EQ(pose.Z(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Roll(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Pitch(), 0.0);
+      EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+    else if (sensorName == "sensorPoseRelative")
+    {
+      foundSensorPoseRelative = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_FLOAT_EQ(pose.X(), 111.0);
+      EXPECT_FLOAT_EQ(pose.Y(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Z(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Roll(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Pitch(), 0.0);
+      EXPECT_NEAR(pose.Yaw(), -1, 1e-5);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+    else if (sensorName == "sensorPoseTwoLevel")
+    {
+      foundSensorPoseTwoLevel = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_FLOAT_EQ(pose.X(), 333.0);
+      EXPECT_FLOAT_EQ(pose.Y(), 111.0);
+      EXPECT_FLOAT_EQ(pose.Z(), 222.0);
+      EXPECT_FLOAT_EQ(pose.Roll(), 0.0);
+      EXPECT_FLOAT_EQ(pose.Pitch(), 0.0);
+      EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+    else if (sensorName == "issue378_sensor")
+    {
+      foundIssue378Sensor = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_FLOAT_EQ(pose.X(), 1);
+      EXPECT_FLOAT_EQ(pose.Y(), 2);
+      EXPECT_FLOAT_EQ(pose.Z(), 3);
+      EXPECT_FLOAT_EQ(pose.Roll(), 0.1);
+      EXPECT_FLOAT_EQ(pose.Pitch(), 0.2);
+      EXPECT_FLOAT_EQ(pose.Yaw(), 0.3);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+    else if (sensorName == "issue67_sensor")
+    {
+      foundIssue67Sensor = true;
+      ASSERT_TRUE(sensor->HasElement("pose"));
+      const auto poseElem = sensor->GetElement("pose");
+
+      const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
+        "", ignition::math::Pose3d::Zero);
+      ASSERT_TRUE(posePair.second);
+
+      const auto& pose = posePair.first;
+
+      EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1);
+      EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1);
+      EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1);
+      EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1);
+      EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1);
+      EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1);
+
+      EXPECT_FALSE(poseElem->GetNextElement("pose"));
+    }
+  }
+
+  EXPECT_TRUE(foundSensorNoPose);
+  EXPECT_TRUE(foundSensorPose);
+  EXPECT_TRUE(foundSensorPoseRelative);
+  EXPECT_TRUE(foundSensorPoseTwoLevel);
+  EXPECT_TRUE(foundIssue378Sensor);
+  EXPECT_TRUE(foundIssue67Sensor);
 }

test/integration/urdf_gazebo_extensions.urdf

@@ -137,4 +137,182 @@
     </visual>
   </link>
 
+  <!-- Test lumping of sensors with <pose> tags. -->
+
+  <link name="linkSensorNoPose">
+    <inertial>
+      <mass value="100"/>
+      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+    </inertial>
+  </link>
+  <joint name="jointSensorNoPose" type="fixed">
+    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
+    <parent link="link0"/>
+    <child link="linkSensorNoPose"/>
+  </joint>
+  <gazebo reference="linkSensorNoPose">
+    <sensor name="sensorNoPose" type="camera">
+      <update_rate>6.0</update_rate>
+      <camera name="cam">
+        <horizontal_fov>1.36869112579</horizontal_fov>
+        <image>
+          <width>1232</width>
+          <height>1616</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
+
+  <link name="linkSensorPose">
+    <inertial>
+      <mass value="100"/>
+      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+    </inertial>
+  </link>
+  <joint name="jointSensorPose" type="fixed">
+    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
+    <parent link="link0"/>
+    <child link="linkSensorPose"/>
+  </joint>
+  <gazebo reference="linkSensorPose">
+    <sensor name="sensorPose" type="camera">
+      <pose>111 0 0 0 0 -1</pose>
+      <update_rate>6.0</update_rate>
+      <camera name="cam">
+        <horizontal_fov>1.36869112579</horizontal_fov>
+        <image>
+          <width>1232</width>
+          <height>1616</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
+
+  <link name="linkSensorPoseRelative">
+    <inertial>
+      <mass value="100"/>
+      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+    </inertial>
+  </link>
+  <joint name="jointSensorPoseRelative" type="fixed">
+    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
+    <parent link="link0"/>
+    <child link="linkSensorPoseRelative"/>
+  </joint>
+  <gazebo reference="linkSensorPoseRelative">
+    <sensor name="sensorPoseRelative" type="camera">
+      <pose relative_to="link0">111 0 0 0 0 -1</pose>
+      <update_rate>6.0</update_rate>
+      <camera name="cam">
+        <horizontal_fov>1.36869112579</horizontal_fov>
+        <image>
+          <width>1232</width>
+          <height>1616</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
+
+  <link name="linkSensorPoseTwoLevel">
+    <inertial>
+      <mass value="100"/>
+      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+    </inertial>
+  </link>
+  <joint name="jointSensorPoseTwoLevel" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 222"/>
+    <parent link="link0"/>
+    <child link="linkSensorPoseTwoLevel"/>
+  </joint>
+  <link name="linkSensorPoseTwoLevel2">
+    <inertial>
+      <mass value="100"/>
+      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+    </inertial>
+  </link>
+  <joint name="jointSensorPoseTwoLevel2" type="fixed">
+    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
+    <parent link="linkSensorPoseTwoLevel"/>
+    <child link="linkSensorPoseTwoLevel2"/>
+  </joint>
+  <gazebo reference="linkSensorPoseTwoLevel2">
+    <sensor name="sensorPoseTwoLevel" type="camera">
+      <pose twoLevel_to="link0">111 0 0 0 0 -1</pose>
+      <update_rate>6.0</update_rate>
+      <camera name="cam">
+        <horizontal_fov>1.36869112579</horizontal_fov>
+        <image>
+          <width>1232</width>
+          <height>1616</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
+
+  <!-- Issue 378 setting -->
+  <gazebo reference="issue378_link">
+    <sensor name="issue378_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>400</update_rate>
+      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
+    </sensor>
+  </gazebo>
+  <link name="issue378_link"/>
+  <joint name="issue378_link_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <axis xyz="0 0 0"/>
+    <parent link="link0"/>
+    <child link="issue378_link"/>
+  </joint>
+
+  <!-- Issue 67 setting -->
+  <link name="Camera">
+    <inertial>
+      <origin xyz="-7.4418E-06 0.0043274 -0.010112" rpy="0 0 0"/>
+      <mass value="0.10621"/>
+      <inertia ixx="7.3134E-05" ixy="7.9651E-09" ixz="-8.9146E-09"
+              iyy="3.0769E-05" iyz="-3.9082E-06"
+              izz="9.2194E-05"/>
+    </inertial>
+  </link>
+  <joint name="jCamera" type="fixed">
+    <origin xyz="-0.20115 0.42488 0.30943" rpy="1.5708 -0.89012 1.5708"/>
+    <parent link="link0"/>
+    <child link="Camera"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <gazebo reference="Camera">
+    <sensor name="issue67_sensor" type="camera">
+      <visualize>true</visualize>
+      <pose>1 1 1 1.570796 1.570796 1.570796</pose>
+      <camera>
+      </camera>
+    </sensor>
+  </gazebo>
+
 </robot>