diff --git a/tesseract_command_language/src/utils.cpp b/tesseract_command_language/src/utils.cpp index c7d6068e1f..45a8e4516e 100644 --- a/tesseract_command_language/src/utils.cpp +++ b/tesseract_command_language/src/utils.cpp @@ -241,11 +241,15 @@ bool formatJointPosition(const std::vector& joint_names, WaypointPo else if (waypoint.isCartesianWaypoint()) { auto& cwp = waypoint.as(); - if (!cwp.hasSeed()) - throw std::runtime_error("Cartesian waypoint does not have a seed."); - - jv = &(cwp.getSeed().position); - jn = &(cwp.getSeed().joint_names); + if (cwp.hasSeed()) + { + jv = &(cwp.getSeed().position); + jn = &(cwp.getSeed().joint_names); + } + else + { + return false; + } } else { diff --git a/tesseract_command_language/test/command_language_utils_unit.cpp b/tesseract_command_language/test/command_language_utils_unit.cpp index 548a7655f2..97c2264e45 100644 --- a/tesseract_command_language/test/command_language_utils_unit.cpp +++ b/tesseract_command_language/test/command_language_utils_unit.cpp @@ -312,13 +312,13 @@ TEST(TesseractCommandLanguageUtilsUnit, formatJointPositionTests) // NOLINT EXPECT_FALSE(formatJointPosition(joint_names, wp1u_poly)); EXPECT_TRUE(wp1u_poly.as().getSeed().position.isApprox(seed_position)); + WaypointPoly wp2_poly{ wp2 }; + EXPECT_FALSE(formatJointPosition(format_joint_names, wp2_poly)); // NOLINT + // Format is not correct size or invalid joint name EXPECT_ANY_THROW(formatJointPosition({ "joint_1" }, wp0_poly)); // NOLINT EXPECT_ANY_THROW(formatJointPosition({ "joint_3", "joint_1" }, wp0_poly)); // NOLINT - WaypointPoly wp2_poly{ wp2 }; - EXPECT_ANY_THROW(formatJointPosition(format_joint_names, wp2_poly)); // NOLINT - WaypointPoly error_poly; EXPECT_ANY_THROW(formatJointPosition(format_joint_names, error_poly)); // NOLINT }