Fix TaskComposerProblem serialization and equal operator

tesseract-robotics · Dec 13, 2023 · bde7a8e · bde7a8e
1 parent a315913
commit bde7a8e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/tesseract_task_composer/core/src/task_composer_problem.cpp b/tesseract_task_composer/core/src/task_composer_problem.cpp
@@ -46,7 +46,7 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const
   bool equal = true;
   equal &= name == rhs.name;
   equal &= dotgraph == rhs.dotgraph;
-  equal &= input == input;
+  equal &= input == rhs.input;
   return equal;
 }
 
@@ -57,7 +57,7 @@ void TaskComposerProblem::serialize(Archive& ar, const unsigned int /*version*/)
 {
   ar& boost::serialization::make_nvp("name", name);
   ar& boost::serialization::make_nvp("dotgraph", dotgraph);
-  ar& boost::serialization::make_nvp("input", dotgraph);
+  ar& boost::serialization::make_nvp("input", input);
 }
 
 }  // namespace tesseract_planning

diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp
@@ -140,6 +140,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests)  // NOLINT
   auto problem = std::make_unique<TaskComposerProblem>();
   EXPECT_EQ(problem->name, "unset");
   EXPECT_FALSE(problem->dotgraph);
+  EXPECT_TRUE(problem->input.isNull());
+
+  tesseract_common::JointState joint_state;
+  joint_state.joint_names = { "joint_1", "joint_2", "joint_3" };
+  joint_state.position = Eigen::VectorXd::Constant(3, 5);
+  joint_state.velocity = Eigen::VectorXd::Constant(3, 6);
+  joint_state.acceleration = Eigen::VectorXd::Constant(3, 7);
+  joint_state.effort = Eigen::VectorXd::Constant(3, 8);
+  joint_state.time = 100;
+
+  problem->input = joint_state;
 
   // Serialization
   test_suite::runSerializationPointerTest(problem, "TaskComposerProblemTests");