Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improve tesseract_scene_graph code coverage #838

Merged
merged 3 commits into from Dec 27, 2022
Merged

Improve tesseract_scene_graph code coverage #838

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b5fa184
Improve tesseract_scene_graph code coverage
Levi-Armstrong Dec 26, 2022
35456aa
fixup
Levi-Armstrong Dec 27, 2022
3f02b31
fixup
Levi-Armstrong Dec 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 2 additions & 2 deletions tesseract_scene_graph/src/graph.cpp
View file
Open in desktop
Expand Up @@ -174,10 +174,10 @@ bool SceneGraph::addLink(const Link& link, const Joint& joint)
}

if (!addLinkHelper(std::make_shared<Link>(link.clone())))
return false;
return false; // LCOV_EXCL_LINE

if (!addJointHelper(std::make_shared<Joint>(joint.clone())))
return false;
return false; // LCOV_EXCL_LINE

return true;
}
Expand Down
9 changes: 9 additions & 0 deletions tesseract_scene_graph/test/tesseract_scene_graph_joint_unit.cpp
View file
Open in desktop
Expand Up @@ -162,6 +162,11 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphJointUnit) // NOLINT
joint_1.mimic->offset = 0.5;
joint_1.mimic->joint_name = "joint_0";
joint_1.mimic->multiplier = 1.5;
joint_1.safety = std::make_shared<JointSafety>();
joint_1.safety->soft_lower_limit = -0.5;
joint_1.safety->soft_upper_limit = 0.5;
joint_1.safety->k_position = 1.1;
joint_1.safety->k_velocity = 2.5;

EXPECT_EQ(joint_1.getName(), "joint_n1");

Expand All @@ -187,6 +192,10 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphJointUnit) // NOLINT
EXPECT_NEAR(joint_1_clone.mimic->offset, 0.5, 1e-6);
EXPECT_EQ(joint_1_clone.mimic->joint_name, "joint_0");
EXPECT_NEAR(joint_1_clone.mimic->multiplier, 1.5, 1e-6);
EXPECT_NEAR(joint_1_clone.safety->soft_lower_limit, -0.5, 1e-6);
EXPECT_NEAR(joint_1_clone.safety->soft_upper_limit, 0.5, 1e-6);
EXPECT_NEAR(joint_1_clone.safety->k_position, 1.1, 1e-6);
EXPECT_NEAR(joint_1_clone.safety->k_velocity, 2.5, 1e-6);

std::ostringstream s1;
s1 << JointType::FIXED;
Expand Down
91 changes: 89 additions & 2 deletions tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp
View file
Open in desktop
Expand Up @@ -90,10 +90,9 @@ tesseract_scene_graph::SceneGraph createTestSceneGraph()
return g;
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphUnit) // NOLINT
void runTest(tesseract_scene_graph::SceneGraph& g)
{
using namespace tesseract_scene_graph;
SceneGraph g = createTestSceneGraph();

// Check getAdjacentLinkNames Method
std::vector<std::string> adjacent_links = g.getAdjacentLinkNames("link_3");
Expand Down Expand Up @@ -265,6 +264,29 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphUnit) // NOLINT

// Should throw since this is a directory and not a file
EXPECT_ANY_THROW(g.saveDOT(tesseract_common::getTempPath())); // NOLINT
EXPECT_ANY_THROW(g.getVertex("vertex_does_not_exist")); // NOLINT
EXPECT_ANY_THROW(g.getEdge("edge_does_not_exist")); // NOLINT
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphUnit) // NOLINT
{
{
tesseract_scene_graph::SceneGraph g = createTestSceneGraph();
runTest(g);
}

{ // Move Constructor
tesseract_scene_graph::SceneGraph g = createTestSceneGraph();
tesseract_scene_graph::SceneGraph g_move(std::move(g));
runTest(g_move);
}

{ // Move Assignment Constructor
tesseract_scene_graph::SceneGraph g = createTestSceneGraph();
tesseract_scene_graph::SceneGraph g_move;
g_move = std::move(g);
runTest(g_move);
}
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphClearUnit) // NOLINT
Expand Down Expand Up @@ -352,6 +374,30 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphAddLinkJointPairUnit) // NOLIN
checkSceneGraph(g);
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphAddLinkUnit) // NOLINT
{
using namespace tesseract_scene_graph;
SceneGraph g = createTestSceneGraph();

Link replace_link("link_5");

// Add link which already exists but replace is not allowed
EXPECT_FALSE(g.addLink(replace_link));
EXPECT_EQ(g.getLinks().size(), 5);
EXPECT_EQ(g.getJoints().size(), 4);

// Check Graph
checkSceneGraph(g);

// Add link which already exists but replace is not allowed
EXPECT_TRUE(g.addLink(replace_link, true));
EXPECT_EQ(g.getLinks().size(), 5);
EXPECT_EQ(g.getJoints().size(), 4);

// Check Graph
checkSceneGraph(g);
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphRemoveLinkUnit) // NOLINT
{
using namespace tesseract_scene_graph;
Expand Down Expand Up @@ -650,6 +696,44 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphAddJointUnit) // NOLINT
EXPECT_FALSE(g.addJoint(joint_4));
EXPECT_EQ(g.getLinks().size(), 5);
EXPECT_EQ(g.getJoints().size(), 4);

// Add Joint with no joint limits
Link link_6("link_6");
EXPECT_TRUE(g.addLink(link_6));
joint_7.type = JointType::PRISMATIC;
EXPECT_FALSE(g.addJoint(joint_7));
EXPECT_EQ(g.getLinks().size(), 6);
EXPECT_EQ(g.getJoints().size(), 4);

joint_7.type = JointType::CONTINUOUS;
EXPECT_TRUE(g.addJoint(joint_7));
EXPECT_EQ(g.getJoints().size(), 5);
EXPECT_TRUE(g.getJoint("joint_7")->limits != nullptr);
EXPECT_NEAR(g.getJoint("joint_7")->limits->lower, -4 * M_PI, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->upper, 4 * M_PI, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->effort, 0.0, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->velocity, 2.0, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->acceleration, 1.0, 1e-5);

Link link_7("link_7");
EXPECT_TRUE(g.addLink(link_7));
EXPECT_EQ(g.getLinks().size(), 7);
EXPECT_EQ(g.getJoints().size(), 5);

Joint joint_8("joint_8");
joint_8.parent_to_joint_origin_transform.translation()(0) = 1.75;
joint_8.parent_link_name = "link_7";
joint_8.child_link_name = "link_6";
joint_8.type = JointType::CONTINUOUS;
joint_8.limits = std::make_shared<JointLimits>(0, 0, 0, 2, 1);
EXPECT_TRUE(g.addJoint(joint_8));
EXPECT_EQ(g.getJoints().size(), 6);
EXPECT_TRUE(g.getJoint("joint_7")->limits != nullptr);
EXPECT_NEAR(g.getJoint("joint_7")->limits->lower, -4 * M_PI, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->upper, 4 * M_PI, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->effort, 0.0, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->velocity, 2.0, 1e-5);
EXPECT_NEAR(g.getJoint("joint_7")->limits->acceleration, 1.0, 1e-5);
}

TEST(TesseractSceneGraphUnit, TesseractSceneGraphRemoveJointUnit) // NOLINT
Expand Down Expand Up @@ -1502,6 +1586,9 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphKDLConversions) // NOLINT
EXPECT_DOUBLE_EQ(kdl_t(static_cast<unsigned>(i), static_cast<unsigned>(j)), t(i, j));
}
}

Eigen::MatrixXd t_wrong_size = Eigen::MatrixXd::Random(7, 7);
EXPECT_ANY_THROW(tesseract_scene_graph::convert(t_wrong_size)); // NOLINT
}

{ // KDL to Eigen
Expand Down