Fix composite and move instruction equal operator and serialization (#…

…561)
tesseract-robotics · Jan 3, 2025 · bd6b13e · bd6b13e
1 parent 73ba1b8
commit bd6b13e
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 5 deletions.
diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp
@@ -28,6 +28,10 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <stdexcept>
 #include <iostream>
+#include <boost/version.hpp>
+#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500)
+#include <boost/serialization/library_version_type.hpp>
+#endif
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
 #include <boost/serialization/unordered_map.hpp>
@@ -227,7 +231,10 @@ bool CompositeInstruction::operator==(const CompositeInstruction& rhs) const
 {
   bool equal = true;
   equal &= (static_cast<int>(order_) == static_cast<int>(rhs.order_));
+  equal &= (uuid_ == rhs.uuid_);
+  equal &= (parent_uuid_ == rhs.parent_uuid_);
   equal &= (profile_ == rhs.profile_);  // NOLINT
+  equal &= (profile_overrides_ == rhs.profile_overrides_);
   equal &= (manipulator_info_ == rhs.manipulator_info_);
   equal &= (user_data_ == rhs.user_data_);
   equal &= (container_.size() == rhs.container_.size());
@@ -530,6 +537,7 @@ void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/
   ar& boost::serialization::make_nvp("description", description_);
   ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_);
   ar& boost::serialization::make_nvp("profile", profile_);
+  ar& boost::serialization::make_nvp("profile_overrides", profile_overrides_);
   ar& boost::serialization::make_nvp("order", order_);
   ar& boost::serialization::make_nvp("user_data", user_data_);
   ar& boost::serialization::make_nvp("container", container_);

diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp
@@ -28,9 +28,14 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <iostream>
 #include <console_bridge/console.h>
+#include <boost/version.hpp>
+#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500)
+#include <boost/serialization/library_version_type.hpp>
+#endif
 #include <boost/uuid/uuid_generators.hpp>
 #include <boost/uuid/uuid_io.hpp>
 #include <boost/uuid/uuid_serialize.hpp>
+#include <boost/serialization/unordered_map.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/move_instruction.h>
@@ -241,12 +246,15 @@ StateWaypointPoly MoveInstruction::createStateWaypoint() { return StateWaypoint(
 bool MoveInstruction::operator==(const MoveInstruction& rhs) const
 {
   bool equal = true;
+  equal &= (uuid_ == rhs.uuid_);
+  equal &= (parent_uuid_ == rhs.parent_uuid_);
   equal &= (static_cast<int>(move_type_) == static_cast<int>(rhs.move_type_));
-  equal &= (waypoint_ == rhs.waypoint_);
-  equal &= (manipulator_info_ == rhs.manipulator_info_);
   equal &= (profile_ == rhs.profile_);            // NO LINT
   equal &= (path_profile_ == rhs.path_profile_);  // NO LINT
-  /** @todo Add profiles overrides when serialization is supported for profiles */
+  equal &= (profile_overrides_ == rhs.profile_overrides_);
+  equal &= (path_profile_overrides_ == rhs.path_profile_overrides_);
+  equal &= (waypoint_ == rhs.waypoint_);
+  equal &= (manipulator_info_ == rhs.manipulator_info_);
   return equal;
 }
 // LCOV_EXCL_START
@@ -262,9 +270,10 @@ void MoveInstruction::serialize(Archive& ar, const unsigned int /*version*/)
   ar& boost::serialization::make_nvp("description", description_);
   ar& boost::serialization::make_nvp("profile", profile_);
   ar& boost::serialization::make_nvp("path_profile", path_profile_);
+  ar& boost::serialization::make_nvp("profile_overrides", profile_overrides_);
+  ar& boost::serialization::make_nvp("path_profile_overrides", path_profile_overrides_);
   ar& boost::serialization::make_nvp("waypoint", waypoint_);
   ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_);
-  /** @todo Add profiles overrides when serialization is supported for profiles */
 }
 
 }  // namespace tesseract_planning

diff --git a/tesseract_command_language/test/command_language_unit.cpp b/tesseract_command_language/test/command_language_unit.cpp
@@ -465,6 +465,8 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests)  // NOLINT
   EXPECT_TRUE(insert_program != instr);
 
   T assign_program(profile, order, manip_info);
+  assign_program.setUUID(instr.getUUID());
+  assign_program.setParentUUID(instr.getParentUUID());
   assign_program.setInstructions(instr.getInstructions());
   EXPECT_FALSE(assign_program.getUUID().is_nil());
   EXPECT_TRUE(assign_program.getParentUUID().is_nil());

diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp
@@ -783,7 +783,12 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsResultTaskTests)
   {  // Test run method
     auto data = std::make_unique<TaskComposerDataStorage>();
     CompositeInstruction compare = test_suite::jointInterpolateExampleProgramABB(false);
-    data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true));
+    CompositeInstruction input_data = test_suite::jointInterpolateExampleProgramABB(true);
+    input_data.setUUID(compare.getUUID());
+    for (std::size_t i = 0; i < compare.size(); ++i)
+      input_data.at(i).as<MoveInstructionPoly>().setUUID(compare.at(i).as<MoveInstructionPoly>().getUUID());
+
+    data->setData("input_data", input_data);
     auto context = std::make_unique<TaskComposerContext>("abc", std::move(data));
     std::vector<std::string> input_keys{ "input_data" };
     std::vector<std::string> output_keys{ "output_data" };