Using degrees boolean attribute in pose instead, added tests and test…

… sdf

Signed-off-by: Aaron Chong <[email protected]>

sdf/1.9/pose.sdf

@@ -1,19 +1,16 @@
 <!-- Pose -->
 <element name="pose" type="pose" default="0 0 0 1 0 0 0" required="0">
-  <description>A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute.</description>
+  <description>A position(x,y,z) and orientation(rpy or wxyz) with respect to the frame named in the relative_to attribute.</description>
 
   <attribute name="relative_to" type="string" default="" required="*">
     <description>
       Name of frame relative to which the pose is applied.
     </description>
   </attribute>
 
-  <attribute name="rotation_type" type="string" default="rpy_radians" required="0">
+  <attribute name="degrees" type="bool" default="false" required="0">
     <description>
-      The type of rotation convention, which must be one of the following:
-      (rpy_degrees) rotation in the form of roll, pitch, yaw, in degrees.
-      (rpy_radians) rotation in the form of roll, pitch, yaw, in radians.
-      (q_wxyz) rotation in the form of a quaternion (w, x, y, z).
+      Whether or not the euler rotations should be read as degrees instead of radians.
     </description>
   </attribute>
 

src/Utils.cc

@@ -60,9 +60,8 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose,
   std::pair<std::string, bool> framePair =
       sdf->Get<std::string>("relative_to", "");
 
-  // Read the rotation type.
-  std::pair<std::string, bool> rotationTypePair =
-      sdf->Get<std::string>("rotation_type", "rpy_radians");
+  // Read the degrees attribute.
+  std::pair<bool, bool> isDegreesPair = sdf->Get<bool>("degrees", false);
 
   // Read the pose value.
   std::pair<ignition::math::Pose3d, bool> posePair =
@@ -74,7 +73,7 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose,
     _frame = framePair.first;
 
     // Modify the rotation component if the unit used was degrees
-    if (rotationTypePair.second && rotationTypePair.first == "rpy_degrees")
+    if (isDegreesPair.second && isDegreesPair.first)
     {
       ignition::math::Vector3d rpyRadians(
           IGN_DTOR(posePair.first.Roll()),

test/integration/CMakeLists.txt

@@ -37,6 +37,7 @@ set(tests
   plugin_attribute.cc
   plugin_bool.cc
   plugin_include.cc
+  pose_1_9_sdf.cc
   provide_feedback.cc
   root_dom.cc
   scene_dom.cc

test/integration/pose_1_9_sdf.cc

@@ -0,0 +1,94 @@
+/*
+ * Copyright 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <string>
+#include <gtest/gtest.h>
+
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+#include "sdf/Element.hh"
+#include "sdf/Error.hh"
+#include "sdf/Model.hh"
+#include "sdf/Root.hh"
+#include "sdf/World.hh"
+#include "sdf/Filesystem.hh"
+#include "test_config.h"
+
+//////////////////////////////////////////////////
+TEST(Pose1_9, ModelPoses)
+{
+  using Pose = ignition::math::Pose3d;
+
+  const std::string testFile = sdf::testing::TestFile(
+      "sdf", "pose_1_9.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  for (const auto e : errors)
+    std::cout << e << std::endl;
+  // std::cout << errors << std::endl;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version());
+
+  const sdf::World *world = root.WorldByIndex(0);
+  ASSERT_NE(nullptr, world);
+
+  std::cout << "model_with_empty_pose" << std::endl;
+  const sdf::Model *model = world->ModelByIndex(0);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_empty_pose", model->Name());
+  EXPECT_EQ(Pose::Zero, model->RawPose());
+
+  std::cout << "model_with_empty_pose_with_attribute_true" << std::endl;
+  model = world->ModelByIndex(1);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_empty_pose_with_attribute_true", model->Name());
+  EXPECT_EQ(Pose::Zero, model->RawPose());
+
+  std::cout << "model_with_empty_pose_with_attribute_false" << std::endl;
+  model = world->ModelByIndex(2);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_empty_pose_with_attribute_false", model->Name());
+  EXPECT_EQ(Pose::Zero, model->RawPose());
+
+  std::cout << "model_with_rpy_pose_no_attribute" << std::endl;
+  model = world->ModelByIndex(3);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_rpy_pose_no_attribute", model->Name());
+  EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose());
+
+  std::cout << "model_with_rpy_pose_with_attribute_true" << std::endl;
+  model = world->ModelByIndex(4);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_rpy_pose_with_attribute_true", model->Name());
+  EXPECT_EQ(Pose(1, 2, 3, IGN_DTOR(0.4), IGN_DTOR(0.5), IGN_DTOR(0.6)),
+      model->RawPose());
+
+  std::cout << "model_with_rpy_pose_with_attribute_false" << std::endl;
+  model = world->ModelByIndex(5);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_rpy_pose_with_attribute_false", model->Name());
+  EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose());
+
+  std::cout << "model_with_quaternion" << std::endl;
+  model = world->ModelByIndex(6);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_quaternion", model->Name());
+  EXPECT_EQ(Pose(1, 2, 3, 0.7071068, 0.7071068, 0, 0), model->RawPose());
+}

test/sdf/pose_1_9.sdf

@@ -0,0 +1,41 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="default">
+
+    <model name="model_with_empty_pose">
+      <pose></pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_pose_with_attribute_true">
+      <pose degrees="true"></pose> 
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_pose_with_attribute_false">
+      <pose degrees="false"></pose> 
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_rpy_pose_no_attribute">
+      <pose>1 2 3 0.4 0.5 0.6</pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_rpy_pose_with_attribute_true">
+      <pose degrees="true">1 2 3 0.4 0.5 0.6</pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_rpy_pose_with_attribute_false">
+      <pose degrees="false">1 2 3 0.4 0.5 0.6</pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_quaternion">
+      <pose>1 2 3 0.7071068 0.7071068 0 0</pose>
+      <link name="link"/>
+    </model>
+
+  </world>
+</sdf>