Skip to content

Commit

Permalink
Improve tesseract_scene_graph code coverage (#838)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 27, 2022
1 parent 49c5dd9 commit 31cb5d7
Show file tree
Hide file tree
Showing 3 changed files with 100 additions and 4 deletions.
4 changes: 2 additions & 2 deletions tesseract_scene_graph/src/graph.cpp
View file
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
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
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

0 comments on commit 31cb5d7

Please sign in to comment.