Skip to content

Commit

Permalink
Param parsing rotation_format
Browse files Browse the repository at this point in the history 
Signed-off-by: Aaron Chong <[email protected]>
  • Loading branch information
@aaronchongth
aaronchongth committed Sep 14, 2021
1 parent c30d086 commit 73a1a49
Show file tree
Hide file tree
Showing 3 changed files with 83 additions and 24 deletions.
10 changes: 5 additions & 5 deletions sdf/1.9/pose.sdf
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<!-- Pose -->
<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
<description>
A pose (x, y, z, r, p, y) expressed in the frame named by @relative_to. The first three components (x, y, z) represent the position of the element's origin (in the @relative_to frame) and the last three components (roll, pitch, yaw) represent the orientation of the element as a sequence of intrinsic Euler rotations.
A pose composed of (translation, rotation) components, expressed in the framenamed by @relative_to. The translation component, (x, y, z) represent the position of the element's origin (in the @relative_to frame). The rotation component represent the orientation of the element as either a sequence of intrinsic Euler rotations (roll, pitch, yaw), or as quaternions (w, x, y, z), where w is the real component.
</description>

<attribute name="relative_to" type="string" default="" required="0">
Expand All @@ -10,11 +10,11 @@
</description>
</attribute>

<attribute name="rotation_format" type="string" default="rpy" required="0">
<attribute name="rotation_format" type="string" default="euler_rpy" required="0">
<description>
rpy by default. Supported rotation formats are
rpy, Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values.
q_wxyz, Quaternion representation in q, w, x, y, z. The pose is expected to have 7 values.
euler_rpy by default. Supported rotation formats are
euler_rpy, Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values.
quat_wxyz, Quaternion representation in w, x, y, z. The pose is expected to have 7 values.
</description>
</attribute>

Expand Down
69 changes: 53 additions & 16 deletions src/Param.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -451,7 +451,7 @@ bool ParsePoseUsingStringStream(const std::string &_input,
{
StringStreamClassicLocale ss(_input);
std::string token;
std::array<double, 6> values;
std::array<double, 7> values;
std::size_t valueIndex = 0;
double v;
bool isValidPose = true;
Expand Down Expand Up @@ -485,9 +485,9 @@ bool ParsePoseUsingStringStream(const std::string &_input,
break;
}

if (valueIndex >= 6u)
if (valueIndex >= 7u)
{
sdferr << "Pose values can only accept 6 values.\n";
sdferr << "Pose values can not accept more than 7 values.\n";
isValidPose = false;
break;
}
Expand All @@ -498,40 +498,77 @@ bool ParsePoseUsingStringStream(const std::string &_input,
if (!isValidPose)
return false;

if (valueIndex != 6u)
{
sdferr << "The value for //pose must have 6 values, but "
<< valueIndex << " were found instead.\n";
return false;
}

const bool defaultParseAsDegrees = false;
bool parseAsDegrees = defaultParseAsDegrees;

const std::string defaultRotationFormat = "euler_rpy";
std::string rotationFormat = defaultRotationFormat;

for (const auto &p : _attributes)
{
if (p->GetKey() == "degrees")
const std::string key = p->GetKey();

if (key == "degrees")
{
if (!p->Get<bool>(parseAsDegrees))
{
sdferr << "Invalid boolean value found for attribute "
"//pose[@degrees].\n";
return false;
}
break;
}
else if (key == "rotation_format")
{
rotationFormat = p->GetAsString();
}
}

if (parseAsDegrees)
auto isDesiredSize =
[&valueIndex](const std::string &_format, std::size_t _size)
{
if (valueIndex != _size)
{
sdferr << "The value for //pose[@rotation_format='" << _format
<< "'] must have " << _size << " values, but " << valueIndex
<< " were found instead.\n";
return false;
}
return true;
};

if (rotationFormat == "euler_rpy" && isDesiredSize(rotationFormat, 6u))
{
if (parseAsDegrees)
{
_value = ignition::math::Pose3d(values[0], values[1], values[2],
IGN_DTOR(values[3]), IGN_DTOR(values[4]), IGN_DTOR(values[5]));
}
else
{
_value = ignition::math::Pose3d(values[0], values[1], values[2],
values[3], values[4], values[5]);
}
}
else if (rotationFormat == "quat_wxyz" && isDesiredSize(rotationFormat, 7u))
{
if (parseAsDegrees)
{
sdferr << "The attribute //pose[@degrees='true'] does not apply when the "
<< "parsing quaternions, //pose[@rotation_format='quat_wxyz'].\n";
return false;
}

_value = ignition::math::Pose3d(values[0], values[1], values[2],
IGN_DTOR(values[3]), IGN_DTOR(values[4]), IGN_DTOR(values[5]));
values[3], values[4], values[5], values[6]);
}
else
{
_value = ignition::math::Pose3d(values[0], values[1], values[2],
values[3], values[4], values[5]);
sdferr << "Undefined attribute //pose[@rotation_format='"
<< rotationFormat << "'], only 'euler_rpy' and 'quat_wxyz'"
<< " is supported.\n";
return false;
}

return true;
}

Expand Down
28 changes: 25 additions & 3 deletions test/integration/pose_1_9_sdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ static bool contains(const std::string &_a, const std::string &_b)
}

//////////////////////////////////////////////////
TEST(Pose1_9, PoseSet7ValuesFail)
TEST(Pose1_9, PoseSet8ValuesFail)
{
// Redirect sdferr output
std::stringstream buffer;
Expand All @@ -128,12 +128,13 @@ TEST(Pose1_9, PoseSet7ValuesFail)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
EXPECT_FALSE(poseValueParam->SetFromString(
"1 2 3 0.7071068 0.7071068 0 0"));
EXPECT_PRED2(contains, buffer.str(), "can only accept 6 values");
"1 2 3 0.7071068 0.7071068 0 0 0"));
EXPECT_PRED2(contains, buffer.str(), "can not accept more than 7 values");
}

//////////////////////////////////////////////////
Expand All @@ -146,6 +147,7 @@ TEST(Pose1_9, PoseElementSetAndGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);
poseElem->Set<Pose>(Pose(1, 2, 3, 0.4, 0.5, 0.6));

Pose elemVal;
Expand All @@ -163,6 +165,7 @@ TEST(Pose1_9, PoseElementSetAndParamGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);
poseElem->Set<Pose>(Pose(1, 2, 3, 0.4, 0.5, 0.6));

sdf::ParamPtr poseValueParam = poseElem->GetValue();
Expand All @@ -183,6 +186,7 @@ TEST(Pose1_9, PoseParamSetAndGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
Expand All @@ -204,6 +208,7 @@ TEST(Pose1_9, PoseParamSetFromStringAndGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
Expand All @@ -225,6 +230,7 @@ TEST(Pose1_9, PoseParamSetAndElemGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
Expand All @@ -246,6 +252,7 @@ TEST(Pose1_9, PoseParamSetAndParentElemGet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
Expand Down Expand Up @@ -274,19 +281,24 @@ TEST(Pose1_9, ChangingParentPoseElementAfterSet)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);
ASSERT_TRUE(poseElem->Set<Pose>(Pose(1, 2, 3, 0.4, 0.5, 0.6)));

sdf::ElementPtr degreesPoseElem(new sdf::Element);
degreesPoseElem->SetName("pose");
degreesPoseElem->AddValue("pose", "0 0 0 0 0 0", true);
degreesPoseElem->AddAttribute("relative_to", "string", "", false);
degreesPoseElem->AddAttribute("degrees", "bool", "true", false);
degreesPoseElem->AddAttribute(
"rotation_format", "string", "euler_rpy", false);

sdf::ElementPtr radiansPoseElem(new sdf::Element);
radiansPoseElem->SetName("pose");
radiansPoseElem->AddValue("pose", "0 0 0 0 0 0", true);
radiansPoseElem->AddAttribute("relative_to", "string", "", false);
radiansPoseElem->AddAttribute("degrees", "bool", "false", false);
radiansPoseElem->AddAttribute(
"rotation_format", "string", "euler_rpy", false);

// Param from original default attibute
sdf::ParamPtr valParam = poseElem->GetValue();
Expand Down Expand Up @@ -325,18 +337,23 @@ TEST(Pose1_9, ChangingParentPoseElementAfterParamSetFromString)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ElementPtr degreesPoseElem(new sdf::Element);
degreesPoseElem->SetName("pose");
degreesPoseElem->AddValue("pose", "0 0 0 0 0 0", true);
degreesPoseElem->AddAttribute("relative_to", "string", "", false);
degreesPoseElem->AddAttribute("degrees", "bool", "true", false);
degreesPoseElem->AddAttribute(
"rotation_format", "string", "euler_rpy", false);

sdf::ElementPtr radiansPoseElem(new sdf::Element);
radiansPoseElem->SetName("pose");
radiansPoseElem->AddValue("pose", "0 0 0 0 0 0", true);
radiansPoseElem->AddAttribute("relative_to", "string", "", false);
radiansPoseElem->AddAttribute("degrees", "bool", "false", false);
radiansPoseElem->AddAttribute(
"rotation_format", "string", "euler_rpy", false);

// Param from original default attibute
sdf::ParamPtr valParam = poseElem->GetValue();
Expand Down Expand Up @@ -373,6 +390,7 @@ TEST(Pose1_9, ChangingAttributeOfParentElement)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

// Param value in radians
sdf::ParamPtr valParam = poseElem->GetValue();
Expand Down Expand Up @@ -410,6 +428,7 @@ TEST(Pose1_9, ToStringWithoutAttrib)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr poseValueParam = poseElem->GetValue();
ASSERT_NE(nullptr, poseValueParam);
Expand All @@ -427,6 +446,7 @@ TEST(Pose1_9, ToStringWithDegreesFalse)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees");
ASSERT_NE(nullptr, degreesAttrib);
Expand All @@ -449,6 +469,7 @@ TEST(Pose1_9, ToStringWithDegreesTrue)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees");
ASSERT_NE(nullptr, degreesAttrib);
Expand All @@ -473,6 +494,7 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute)
poseElem->AddValue("pose", "0 0 0 0 0 0", true);
poseElem->AddAttribute("relative_to", "string", "", false);
poseElem->AddAttribute("degrees", "bool", "false", false);
poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false);

// Param value in radians
sdf::ParamPtr valParam = poseElem->GetValue();
Expand Down

0 comments on commit 73a1a49

Please sign in to comment.