Skip to content

Commit

Permalink
Backport test for URDF sensor poses
Browse files Browse the repository at this point in the history 
Backported from #525.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters committed Aug 20, 2022
1 parent 3e76d92 commit 16ff849
Show file tree
Hide file tree
Showing 2 changed files with 337 additions and 0 deletions.
159 changes: 159 additions & 0 deletions test/integration/urdf_gazebo_extensions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,4 +135,163 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
EXPECT_FALSE(link->HasElement("velocity_decay"));
}
}

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;
EXPECT_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_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 0.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_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;
EXPECT_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_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 111.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_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;
EXPECT_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_DOUBLE_EQ(pose.X(), 111.0);
EXPECT_DOUBLE_EQ(pose.Y(), 0.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0);
EXPECT_NEAR(pose.Yaw(), -1, 1e-5);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "sensorPoseTwoLevel")
{
foundSensorPoseTwoLevel = true;
EXPECT_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_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 111.0);
EXPECT_DOUBLE_EQ(pose.Z(), 222.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_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;
EXPECT_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_DOUBLE_EQ(pose.X(), 1);
EXPECT_DOUBLE_EQ(pose.Y(), 2);
EXPECT_DOUBLE_EQ(pose.Z(), 3);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.1);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2);
EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "issue67_sensor")
{
foundIssue67Sensor = true;
EXPECT_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);
}
178 changes: 178 additions & 0 deletions test/integration/urdf_gazebo_extensions.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,4 +150,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>

0 comments on commit 16ff849

Please sign in to comment.