improve parsing of xyz and rpy fields

robot_calibration/src/optimization/offsets.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2023 Michael Ferguson
+ * Copyright (C) 2018-2024 Michael Ferguson
  * Copyright (C) 2013-2014 Unbounded Robotics Inc.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -290,13 +290,18 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
         const char * xyz_str = origin_xml->Attribute("xyz");
         const char * rpy_str = origin_xml->Attribute("rpy");
 
+        // Helper to remove empty strings in case there are extra spaces in xyz/rpy fields
+        auto is_empty = [](const std::string &s) { return s.empty(); };
+
         // Split out xyz of origin, break into 3 strings
         std::vector<std::string> xyz_pieces;
-        boost::split(xyz_pieces, xyz_str, boost::is_any_of(" "));
+        if (xyz_str) boost::split(xyz_pieces, xyz_str, boost::is_any_of(" "));
+        xyz_pieces.erase(std::remove_if(std::begin(xyz_pieces), std::end(xyz_pieces), is_empty), std::end(xyz_pieces));
 
         // Split out rpy of origin, break into 3 strings
         std::vector<std::string> rpy_pieces;
-        boost::split(rpy_pieces, rpy_str, boost::is_any_of(" "));
+        if (rpy_str) boost::split(rpy_pieces, rpy_str, boost::is_any_of(" "));
+        rpy_pieces.erase(std::remove_if(std::begin(rpy_pieces), std::end(rpy_pieces), is_empty), std::end(rpy_pieces));
 
         KDL::Frame origin(KDL::Rotation::Identity(), KDL::Vector::Zero());
         if (xyz_pieces.size() == 3)

robot_calibration/test/optimization_offsets_tests.cpp

@@ -61,6 +61,32 @@ std::string robot_description_updated =
 "    <link name=\"link_3\"/>\n"
 "</robot>";
 
+// Test with 1) missing RPY, 2) extra space in xyz
+std::string garbled_robot_description =
+"<?xml version='1.0' ?>"
+"<robot name='test'>"
+"  <link name='link_0'/>"
+"  <joint name='first_joint' type='fixed'>"
+"    <origin xyz='1  1 1'/>"
+"    <parent link='link_0'/>"
+"    <child link='link_1'/>"
+"  </joint>"
+"  <link name='link_1'/>"
+"</robot>";
+
+std::string garbled_robot_description_updated =
+"<?xml version='1.0' ?>\n"
+"<robot name=\"test\">\n"
+"    <link name=\"link_0\"/>\n"
+"    <joint name=\"first_joint\" type=\"fixed\">\n"
+"        <origin xyz=\"1.00000000 1.00000000 1.00000000\" rpy=\"0.00000000 1.57000000 0.00000000\"/>\n"
+"        <parent link=\"link_0\"/>\n"
+"        <child link=\"link_1\"/>\n"
+"    </joint>\n"
+"    <link name=\"link_1\"/>\n"
+"</robot>";
+
+
 TEST(OptimizationOffsetTests, test_urdf_update)
 {
   robot_calibration::OptimizationOffsets offsets;
@@ -77,7 +103,7 @@ TEST(OptimizationOffsetTests, test_urdf_update)
   offsets.update(params);
 
   std::string s = offsets.updateURDF(robot_description);
-  
+
   // google test fails if we give it all of robot_description_updated, so break this up
   std::vector<std::string> s_pieces;
   std::vector<std::string> robot_pieces;
@@ -91,6 +117,35 @@ TEST(OptimizationOffsetTests, test_urdf_update)
   }
 }
 
+TEST(OptimizationOffsetTests, test_with_garbled_xyzrpy)
+{
+  robot_calibration::OptimizationOffsets offsets;
+
+  offsets.addFrame("first_joint", true, true, true, true, true, true);
+
+  double params[6] = {0, 0, 0, 0, 0, 0};
+
+  // set angles
+  KDL::Rotation r = KDL::Rotation::RPY(1.57, 0, 0);
+  robot_calibration::axis_magnitude_from_rotation(r, params[4], params[5], params[6]);
+
+  offsets.update(params);
+
+  std::string s = offsets.updateURDF(garbled_robot_description);
+
+  // google test fails if we give it all of robot_description_updated, so break this up
+  std::vector<std::string> s_pieces;
+  std::vector<std::string> robot_pieces;
+
+  boost::split(s_pieces, s, boost::is_any_of("\n"));
+  boost::split(robot_pieces, garbled_robot_description_updated, boost::is_any_of("\n"));
+
+  for (size_t i = 0; i < robot_pieces.size(); ++i)
+  {
+    ASSERT_STREQ(robot_pieces[i].c_str(), s_pieces[i].c_str());
+  }
+}
+
 TEST(OptimizationOffsetsTests, test_multi_step)
 {
   robot_calibration::OptimizationOffsets offsets;