Add test suite to command language and improve code coverage (#331)

tesseract-robotics · Jun 14, 2023 · 4564e7f · 4564e7f
1 parent dd5f9ae
commit 4564e7f
Show file tree

Hide file tree

Showing 31 changed files with 2,942 additions and 1,744 deletions.
diff --git a/.clang-tidy b/.clang-tidy
@@ -33,7 +33,8 @@ Checks: >
     -readability-function-cognitive-complexity,
     -readability-use-anyofallof,
     -readability-identifier-length,
-    -readability-suspicious-call-argument
+    -readability-suspicious-call-argument,
+    -readability-function-size
 WarningsAsErrors: >
     -*,
     clang-diagnostic-*,
@@ -68,7 +69,8 @@ WarningsAsErrors: >
     -readability-function-cognitive-complexity
     -readability-use-anyofallof,
     -readability-identifier-length,
-    -readability-suspicious-call-argument
+    -readability-suspicious-call-argument,
+    -readability-function-size
 HeaderFilterRegex: '.*'
 AnalyzeTemporaryDtors: false
 FormatStyle:     none

diff --git a/tesseract_command_language/include/tesseract_command_language/instruction_type.h b/tesseract_command_language/include/tesseract_command_language/instruction_type.h
@@ -30,10 +30,6 @@ namespace tesseract_planning
 {
 struct InstructionPoly;
 
-bool isCommentInstruction(const InstructionPoly& instruction);
-
-bool isVariableInstruction(const InstructionPoly& instruction);
-
 bool isSetAnalogInstruction(const InstructionPoly& instruction);
 
 bool isSetToolInstruction(const InstructionPoly& instruction);

diff --git a/...d_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp b/...d_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp
@@ -0,0 +1,229 @@
+/**
+ * @file cartesian_waypoint_poly_unit.hpp
+ * @brief Collection of unit tests for CartesianWaypointPoly
+ *
+ * @author Levi Armstrong
+ * @date June 12, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Levi Armstrong
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * 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
+ * @par
+ * 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.
+ */
+#ifndef TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP
+#define TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP
+
+#include <tesseract_command_language/poly/cartesian_waypoint_poly.h>
+#include <tesseract_command_language/test_suite/waypoint_poly_unit.hpp>
+
+namespace tesseract_planning::test_suite
+{
+template <typename T>
+void runCartesianWaypointTest()
+{
+  runWaypointInterfaceTest<T>();
+
+  {  // Cartesian WaypointPoly Interface Test
+    const std::string name{ "tesseract_planning::test_suite::WaypointPoly" };
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_FALSE(wp.isNull());
+    EXPECT_NE(name, wp.getName());
+    wp.setName(name);
+    EXPECT_EQ(name, wp.getName());
+    EXPECT_NO_THROW(wp.print());         // NOLINT
+    EXPECT_NO_THROW(wp.print("test_"));  // NOLINT
+    EXPECT_TRUE(wp.getType() == std::type_index(typeid(T)));
+
+    WaypointPoly base = wp;
+    EXPECT_TRUE(base.isCartesianWaypoint());
+    EXPECT_FALSE(base.isJointWaypoint());
+    EXPECT_FALSE(base.isStateWaypoint());
+  }
+
+  {  // Test default construction
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_TRUE(wp.getTransform().isApprox(Eigen::Isometry3d::Identity()));
+    EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0);
+    EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0);
+    EXPECT_FALSE(wp.hasSeed());
+    EXPECT_FALSE(wp.isToleranced());
+  }
+
+  {   // Set/Get Transform
+    { // Test construction providing pose
+      Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
+  pose.translation() = Eigen::Vector3d(1, 2, 3);
+  CartesianWaypointPoly wp{ T(pose) };
+  EXPECT_TRUE(wp.getTransform().isApprox(pose));
+  EXPECT_FALSE(wp.isToleranced());
+}
+
+{  // Test construction providing pose
+  Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
+  pose.translation() = Eigen::Vector3d(1, 2, 3);
+  CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) };
+  EXPECT_TRUE(wp.getTransform().isApprox(pose));
+  EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5)));
+  EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5)));
+  EXPECT_TRUE(wp.isToleranced());
+}
+
+{  // Test set pose
+  Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
+  pose.translation() = Eigen::Vector3d(1, 2, 3);
+  CartesianWaypointPoly wp{ T() };
+  wp.setTransform(pose);
+  EXPECT_TRUE(wp.getTransform().isApprox(pose));
+  EXPECT_FALSE(wp.isToleranced());
+}
+
+{  // Test assigning pose
+  Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
+  pose.translation() = Eigen::Vector3d(1, 2, 3);
+  CartesianWaypointPoly wp{ T() };
+  wp.getTransform() = pose;
+  EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose));
+  EXPECT_FALSE(wp.isToleranced());
+}
+}  // namespace tesseract_planning::test_suite
+
+{  // Test Set Tolerances
+  CartesianWaypointPoly wp{ T() };
+  EXPECT_FALSE(wp.isToleranced());
+
+  wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
+  wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
+  EXPECT_TRUE(wp.isToleranced());
+
+  wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5));
+  wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
+  EXPECT_ANY_THROW(wp.isToleranced());  // NOLINT
+
+  wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
+  wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5));
+  EXPECT_ANY_THROW(wp.isToleranced());  // NOLINT
+
+  wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0));
+  wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0));
+  EXPECT_FALSE(wp.isToleranced());
+}
+
+{ // Test Equality and Serialization
+  { CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+CartesianWaypointPoly wp2(wp1);  // NOLINT
+EXPECT_TRUE(wp1 == wp2);
+EXPECT_TRUE(wp2 == wp1);
+EXPECT_FALSE(wp2 != wp1);
+runWaypointSerializationTest(wp1);
+}
+{
+  CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+  wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
+  CartesianWaypointPoly wp2(wp1);
+  EXPECT_TRUE(wp1 == wp2);
+  EXPECT_TRUE(wp2 == wp1);
+  EXPECT_FALSE(wp2 != wp1);
+  runWaypointSerializationTest(wp1);
+}
+{
+  CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+  wp1.getUpperTolerance().resize(3);
+  wp1.getUpperTolerance() << 1, 2, 3;
+  wp1.getLowerTolerance().resize(3);
+  wp1.getLowerTolerance() << -4, -5, -6;
+  CartesianWaypointPoly wp2(wp1);
+  EXPECT_TRUE(wp1 == wp2);
+  EXPECT_TRUE(wp2 == wp1);
+  EXPECT_FALSE(wp2 != wp1);
+  runWaypointSerializationTest(wp1);
+}
+// Not equal
+{
+  CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+  CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
+  wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1)));
+  EXPECT_FALSE(wp1 == wp2);
+  EXPECT_FALSE(wp2 == wp1);
+  EXPECT_TRUE(wp2 != wp1);
+  runWaypointSerializationTest(wp2);
+}
+{
+  CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+  CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
+  wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
+  EXPECT_FALSE(wp1 == wp2);
+  EXPECT_FALSE(wp2 == wp1);
+  EXPECT_TRUE(wp2 != wp1);
+  runWaypointSerializationTest(wp2);
+}
+{
+  CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
+  CartesianWaypointPoly wp2(wp1);
+  wp2.getUpperTolerance().resize(3);
+  wp2.getUpperTolerance() << 1, 2, 3;
+  wp2.getLowerTolerance().resize(3);
+  wp2.getLowerTolerance() << -4, -5, -6;
+  EXPECT_FALSE(wp1 == wp2);
+  EXPECT_FALSE(wp2 == wp1);
+  EXPECT_TRUE(wp2 != wp1);
+  runWaypointSerializationTest(wp2);
+}
+}
+
+{  // Set/Get Seed
+  tesseract_common::JointState seed_state;
+  seed_state.joint_names = { "joint_1", "joint_2", "joint_3" };
+  seed_state.position.resize(3);
+  seed_state.position << .01, .02, .03;
+  seed_state.velocity.resize(3);
+  seed_state.velocity << .1, .2, .3;
+  seed_state.acceleration.resize(3);
+  seed_state.acceleration << 1, 2, 3;
+
+  {  // Test default construction pose
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_FALSE(wp.hasSeed());
+    EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state);
+  }
+
+  {  // Test assigning pose
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_FALSE(wp.hasSeed());
+    wp.setSeed(seed_state);
+    EXPECT_TRUE(wp.hasSeed());
+    EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
+  }
+
+  {  // Test assigning pose
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_FALSE(wp.hasSeed());
+    wp.getSeed() = seed_state;
+    EXPECT_TRUE(wp.hasSeed());
+    EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
+  }
+
+  {  // Test clear seed
+    CartesianWaypointPoly wp{ T() };
+    EXPECT_FALSE(wp.hasSeed());
+    wp.getSeed() = seed_state;
+    EXPECT_TRUE(wp.hasSeed());
+    wp.clearSeed();
+    EXPECT_FALSE(wp.hasSeed());
+  }
+}
+}
+}
+#endif  // TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP
diff --git a/..._command_language/include/tesseract_command_language/test_suite/instruction_poly_unit.hpp b/..._command_language/include/tesseract_command_language/test_suite/instruction_poly_unit.hpp
@@ -0,0 +1,90 @@
+/**
+ * @file cartesian_waypoint_poly_unit.hpp
+ * @brief Collection of unit tests for CartesianWaypointPoly
+ *
+ * @author Levi Armstrong
+ * @date June 12, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Levi Armstrong
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * 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
+ * @par
+ * 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.
+ */
+#ifndef TESSERACT_COMMAND_LANGUAGE_INSTRUCTION_POLY_UNIT_HPP
+#define TESSERACT_COMMAND_LANGUAGE_INSTRUCTION_POLY_UNIT_HPP
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <gtest/gtest.h>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <fstream>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_command_language/poly/instruction_poly.h>
+#include <tesseract_common/serialization.h>
+#include <tesseract_common/utils.h>
+
+namespace tesseract_planning::test_suite
+{
+template <typename T>
+void runInstructionInterfaceTest(bool expect_uuid_null = true)
+{
+  InstructionPoly inst{ T() };
+  auto uuid = inst.getUUID();
+  EXPECT_EQ(uuid.is_nil(), expect_uuid_null);
+  EXPECT_TRUE(inst.getParentUUID().is_nil());
+
+  inst.regenerateUUID();
+  auto reg_uuid = inst.getUUID();
+  EXPECT_FALSE(reg_uuid.is_nil());
+  EXPECT_NE(inst.getUUID(), uuid);
+
+  inst.setParentUUID(reg_uuid);
+  EXPECT_FALSE(inst.getParentUUID().is_nil());
+  EXPECT_EQ(inst.getParentUUID(), reg_uuid);
+
+  const std::string desc{ "tesseract_planning::test_suite::Instruction::Description" };
+  EXPECT_NE(inst.getDescription(), desc);
+  inst.setDescription(desc);
+  EXPECT_EQ(inst.getDescription(), desc);
+
+  EXPECT_NO_THROW(inst.print());         // NOLINT
+  EXPECT_NO_THROW(inst.print("test_"));  // NOLINT
+
+  InstructionPoly assign = inst;
+  EXPECT_TRUE(assign == inst);
+  EXPECT_TRUE(assign.getUUID() == inst.getUUID());
+
+  InstructionPoly copy(inst);
+  EXPECT_TRUE(copy == inst);
+  EXPECT_TRUE(assign.getUUID() == inst.getUUID());
+}
+
+/**
+ * @brief This will test the serialization of any waypoint implementation
+ * @param waypoint The waypoint to test serialization
+ */
+inline void runInstructionSerializationTest(const InstructionPoly& inst)
+{
+  const std::string filepath = tesseract_common::getTempPath() + "instruction_poly_boost.xml";
+  tesseract_common::Serialization::toArchiveFileXML<InstructionPoly>(inst, filepath);
+  auto ninst = tesseract_common::Serialization::fromArchiveFileXML<InstructionPoly>(filepath);
+  EXPECT_TRUE(inst == ninst);
+}
+}  // namespace tesseract_planning::test_suite
+
+#endif  // TESSERACT_COMMAND_LANGUAGE_INSTRUCTION_POLY_UNIT_HPP