Fix formatJointPosition handle of cartesian seed

tesseract-robotics · Dec 25, 2024 · 008d3f8 · 008d3f8
1 parent e00ae6f
commit 008d3f8
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 8 deletions.
diff --git a/tesseract_command_language/src/utils.cpp b/tesseract_command_language/src/utils.cpp
@@ -241,11 +241,15 @@ bool formatJointPosition(const std::vector<std::string>& joint_names, WaypointPo
   else if (waypoint.isCartesianWaypoint())
   {
     auto& cwp = waypoint.as<CartesianWaypointPoly>();
-    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
@@ -312,13 +312,13 @@ TEST(TesseractCommandLanguageUtilsUnit, formatJointPositionTests)  // NOLINT
   EXPECT_FALSE(formatJointPosition(joint_names, wp1u_poly));
   EXPECT_TRUE(wp1u_poly.as<CartesianWaypointPoly>().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
 }