diff --git a/sdf/1.9/pose.sdf b/sdf/1.9/pose.sdf
index 523f0894b..efb969b61 100644
--- a/sdf/1.9/pose.sdf
+++ b/sdf/1.9/pose.sdf
@@ -1,6 +1,6 @@
- A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute.
+ A position(x,y,z) and orientation(roll, pitch, yaw) with respect to the frame named in the relative_to attribute.
@@ -8,4 +8,19 @@
+
+ The position(x,y,z) with respect to the frame named in the relative_to attribute.
+
+
+
+ The rotation with respect to the frame named in the relative_to attribute.
+
+ 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).
+
+
+
+
diff --git a/src/Param.cc b/src/Param.cc
index 4335fa890..fd75782cf 100644
--- a/src/Param.cc
+++ b/src/Param.cc
@@ -427,6 +427,83 @@ bool ParseColorUsingStringStream(const std::string &_input,
return isValidColor;
}
+//////////////////////////////////////////////////
+/// \brief Helper function for Param::ValueFromString for parsing rotations
+/// This checks the rotation components specified in _input are rpy or wxyz
+/// (expects 3 or 4 values) and the resulting quaternion is valid.
+/// \param[in] _input Input string.
+/// \param[in] _key Key of the parameter, used for error message.
+/// \param[out] _value This will be set with the parsed value.
+/// \return True if parsing colors succeeded.
+bool ParseRotationUsingStringStream(const std::string &_input,
+ const std::string &_key, ParamPrivate::ParamVariant &_value)
+{
+ StringStreamClassicLocale ss(_input);
+ std::string token;
+ std::vector values;
+ double v;
+ bool isValidRotation = true;
+ while (ss >> token)
+ {
+ try
+ {
+ v = std::stod(token);
+ }
+ // Catch invalid argument exception from std::stod
+ catch(std::invalid_argument &)
+ {
+ sdferr << "Invalid argument. Unable to set value ["<< token
+ << "] for key [" << _key << "].\n";
+ isValidRotation = false;
+ break;
+ }
+ // Catch out of range exception from std::stof
+ catch(std::out_of_range &)
+ {
+ sdferr << "Out of range. Unable to set value [" << token
+ << "] for key [" << _key << "].\n";
+ isValidRotation = false;
+ break;
+ }
+
+ if (values.size() > 4)
+ {
+ sdferr << "Rotation values cannot have more than 4 values.\n";
+ isValidRotation = false;
+ break;
+ }
+
+ if (!std::isfinite(v))
+ {
+ sdferr << "Rotation values must be finite.\n";
+ isValidRotation = false;
+ break;
+ }
+
+ values.push_back(v);
+ }
+
+ if (!isValidRotation)
+ return false;
+
+ if (values.size() == 3u)
+ {
+ _value = ignition::math::Vector3d(values[0], values[1], values[2]);
+ }
+ else if (values.size() == 4u)
+ {
+ _value = ignition::math::Quaterniond(
+ values[0], values[1], values[2], values[3]);
+ }
+ else
+ {
+ sdferr << "Rotation values must have either 3 or 4 values.\n";
+ return false;
+ }
+
+ return true;
+}
+
//////////////////////////////////////////////////
bool Param::ValueFromString(const std::string &_value)
{
@@ -553,6 +630,11 @@ bool Param::ValueFromString(const std::string &_value)
return ParseUsingStringStream(
tmp, this->dataPtr->key, this->dataPtr->value);
}
+ else if (this->dataPtr->typeName == "rotation")
+ {
+ return ParseRotationUsingStringStream(
+ tmp, this->dataPtr->key, this->dataPtr->value);
+ }
else
{
sdferr << "Unknown parameter type[" << this->dataPtr->typeName << "]\n";
@@ -698,3 +780,4 @@ bool Param::ValidateValue() const
return true;
}, this->dataPtr->value);
}
+
diff --git a/src/Utils.cc b/src/Utils.cc
index 3b2834706..90f9ea2d0 100644
--- a/src/Utils.cc
+++ b/src/Utils.cc
@@ -56,24 +56,82 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose,
return false;
}
- // Read the frame. An empty frame implies the parent frame.
+ // Default zero pose.
+ ignition::math::Pose3d pose(
+ ignition::math::Vector3d::Zero, ignition::math::Quaterniond::Identity);
+
+ // Read the frame. An empty frame implies the parent frame. This is optional.
std::pair framePair =
sdf->Get("relative_to", "");
- // Read the pose value.
- std::pair posePair =
- sdf->Get("", ignition::math::Pose3d::Zero);
+ // Start checking for translation and rotation elements.
+ sdf::ElementPtr translationPtr = sdf->GetElement("translation");
+ sdf::ElementPtr rotationPtr = sdf->GetElement("rotation");
- // Set output, but only if the return value is true.
- if (posePair.second)
+ // If both are not explicitly set, the pose is parsed using the value.
+ if (!translationPtr->GetExplicitlySetInFile() &&
+ !rotationPtr->GetExplicitlySetInFile())
{
+ std::pair posePair =
+ sdf->Get("", ignition::math::Pose3d::Zero);
_pose = posePair.first;
_frame = framePair.first;
+
+ // In this scenario, return true or false based on pose element value.
+ return posePair.second;
+ }
+
+ // Read the translation values.
+ if (translationPtr->GetExplicitlySetInFile())
+ {
+ std::pair translationPair =
+ translationPtr->Get(
+ "", ignition::math::Vector3d::Zero);
+ if (translationPair.second)
+ pose.Set(translationPair.first, pose.Rot());
+ }
+
+ // Read the rotation values.
+ if (rotationPtr->GetExplicitlySetInFile())
+ {
+ std::pair typePair =
+ rotationPtr->Get("type", "");
+ if (!typePair.second)
+ return false;
+
+ if (typePair.first == "rpy_degrees")
+ {
+ std::pair rpyDegPair =
+ rotationPtr->Get(
+ "", ignition::math::Vector3d::Zero);
+ if (rpyDegPair.second)
+ pose.Set(pose.Pos(), IGN_DTOR(rpyDegPair.first));
+ }
+ else if (typePair.first == "rpy_radians")
+ {
+ std::pair rpyRadPair =
+ rotationPtr->Get(
+ "", ignition::math::Vector3d::Zero);
+ if (rpyRadPair.second)
+ pose.Set(pose.Pos(), rpyRadPair.first);
+ }
+ else if (typePair.first == "q_wxyz")
+ {
+ std::pair quatPair =
+ rotationPtr->Get(
+ "", ignition::math::Quaterniond::Identity);
+ if (quatPair.second)
+ {
+ pose.Set(pose.Pos(), quatPair.first);
+ }
+ }
+ else
+ return false;
}
- // The frame attribute is optional, so only return true or false based
- // on the pose element value.
- return posePair.second;
+ _pose = pose;
+ _frame = framePair.first;
+ return true;
}
/////////////////////////////////////////////////
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 849e7b2de..5eda1c932 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -38,6 +38,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
diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc
new file mode 100644
index 000000000..1a393bad5
--- /dev/null
+++ b/test/integration/pose_1_9_sdf.cc
@@ -0,0 +1,179 @@
+/*
+ * 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
+#include
+
+#include
+#include
+#include
+#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 << "modelWithEmptyPose" << 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 << "modelWithPoseValue" << std::endl;
+ model = world->ModelByIndex(1);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_pose_value", model->Name());
+ EXPECT_EQ(Pose(1, 2, 3, 1, 2, 3), model->RawPose());
+
+ std::cout << "modelWithEmptyTranslation" << std::endl;
+ model = world->ModelByIndex(2);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_empty_translation", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithTranslationValue" << std::endl;
+ model = world->ModelByIndex(3);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_translation_value", model->Name());
+ EXPECT_EQ(Pose(2, 3, 4, 0, 0, 0), model->RawPose());
+
+ std::cout << "modelWithEmptyRPYDeg" << std::endl;
+ model = world->ModelByIndex(4);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_empty_rpy_deg", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithRPYDegValue" << std::endl;
+ model = world->ModelByIndex(5);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_rpy_deg_value", model->Name());
+ EXPECT_EQ(Pose(0, 0, 0, 0, IGN_DTOR(90), IGN_DTOR(180)), model->RawPose());
+
+ std::cout << "modelWithEmptyRPYRad" << std::endl;
+ model = world->ModelByIndex(6);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_empty_rpy_rad", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithRPYRadValue" << std::endl;
+ model = world->ModelByIndex(7);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_rpy_rad_value", model->Name());
+ EXPECT_EQ(Pose(0, 0, 0, 1, 2, 3), model->RawPose());
+
+ std::cout << "modelWithEmptyQuat" << std::endl;
+ model = world->ModelByIndex(8);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_empty_quat", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithQuatValue" << std::endl;
+ model = world->ModelByIndex(9);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_quat_value", model->Name());
+ EXPECT_EQ(Pose(0, 0, 0, 0.7071068, 0.7071068, 0, 0), model->RawPose());
+
+ std::cout << "modelWithBothRotationsNoValue" << std::endl;
+ model = world->ModelByIndex(10);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_both_rotations_no_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithBothRotationsSecondValue" << std::endl;
+ model = world->ModelByIndex(11);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_both_rotations_second_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithBothRotationsFirstValue" << std::endl;
+ model = world->ModelByIndex(12);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_both_rotations_first_value", model->Name());
+ EXPECT_EQ(Pose(0, 0, 0, 1, 2, 3), model->RawPose());
+
+ std::cout << "modelWithTranslationRotationNoValue" << std::endl;
+ model = world->ModelByIndex(13);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_translation_rotation_no_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithTranslationRotationValue" << std::endl;
+ model = world->ModelByIndex(14);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_translation_rotation_value", model->Name());
+ EXPECT_EQ(Pose(2, 3, 4, 0.7071068, 0.7071068, 0, 0), model->RawPose());
+
+ std::cout << "modelWithValueAndTranslationNoValue" << std::endl;
+ model = world->ModelByIndex(15);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_translation_no_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithValueAndTranslationValue" << std::endl;
+ model = world->ModelByIndex(16);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_translation_value", model->Name());
+ EXPECT_EQ(Pose(2, 3, 4, 0, 0, 0), model->RawPose());
+
+ std::cout << "modelWithValueAndRotationNoValue" << std::endl;
+ model = world->ModelByIndex(17);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_rotation_no_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithValueAndRotationValue" << std::endl;
+ model = world->ModelByIndex(18);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_rotation_value", model->Name());
+ EXPECT_EQ(Pose(0, 0, 0, IGN_DTOR(1), IGN_DTOR(2), IGN_DTOR(3)),
+ model->RawPose());
+
+ std::cout << "modelWithValueAndTranslationRotationNoValue" << std::endl;
+ model = world->ModelByIndex(19);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_translation_rotation_no_value", model->Name());
+ EXPECT_EQ(Pose::Zero, model->RawPose());
+
+ std::cout << "modelWithValueAndTranslationRotationValue" << std::endl;
+ model = world->ModelByIndex(20);
+ ASSERT_NE(nullptr, model);
+ ASSERT_EQ("model_with_value_and_translation_rotation_value", model->Name());
+ EXPECT_EQ(Pose(2, 3, 4, 0.7071068, 0.7071068, 0, 0), model->RawPose());
+}
+
diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf
new file mode 100644
index 000000000..0aefc03dc
--- /dev/null
+++ b/test/sdf/pose_1_9.sdf
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+ 1 2 3 1 2 3
+
+
+
+
+
+
+
+
+
+
+
+
+ 2 3 4
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 90 180
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1 2 3
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.7071068 0.7071068 0 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.7071068 0.7071068 0 0
+
+
+
+
+
+
+ 1 2 3
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 2 3 4
+ 0.7071068 0.7071068 0 0
+
+
+
+
+
+
+ 1 2 3 1 2 3
+
+
+
+
+
+
+
+ 1 2 3 1 2 3
+ 2 3 4
+
+
+
+
+
+
+ 1 2 3 1 2 3
+
+
+
+
+
+
+
+ 1 2 3 1 2 3
+ 1 2 3
+
+
+
+
+
+
+ 1 2 3 1 2 3
+
+
+
+
+
+
+
+
+ 1 2 3 1 2 3
+ 2 3 4
+ 0.7071068 0.7071068 0 0
+
+
+
+
+
+
+