From a01b550d489ca10c0b0668e32989c32515ecc13e Mon Sep 17 00:00:00 2001
From: Tyler Marr <tylermarr17@gmail.com>
Date: Mon, 19 Jun 2023 14:07:41 -0500
Subject: [PATCH] Better match tesseract_ros per @rjoomen

---
 .../src/tesseract_planning_server.cpp            | 16 +++++++---------
 1 file changed, 7 insertions(+), 9 deletions(-)

diff --git a/tesseract_planning_server/src/tesseract_planning_server.cpp b/tesseract_planning_server/src/tesseract_planning_server.cpp
index e11c9d40..cd59fcde 100644
--- a/tesseract_planning_server/src/tesseract_planning_server.cpp
+++ b/tesseract_planning_server/src/tesseract_planning_server.cpp
@@ -208,12 +208,13 @@ void TesseractPlanningServer::onMotionPlanningCallback(
     return;
   }
 
-  tesseract_planning::CompositeInstruction ci;
+  auto problem = std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(goal->request.name);
 
   try
   {
-    ci = Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions)
-             .as<tesseract_planning::CompositeInstruction>();
+    auto ci = Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions)
+                  .as<tesseract_planning::CompositeInstruction>();
+    problem->input_data.setData(input_key_, ci);
   }
   catch (const std::exception& e)
   {
@@ -235,12 +236,9 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   env->applyCommands(tesseract_rosutils::fromMsg(goal->request.commands));
   env->setState(env_state.joints);
 
-  tesseract_planning::TaskComposerDataStorage input_data;
-  input_data.setData(input_key_, ci);
-  auto problem = std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(
-      env, input_data, profiles_);  //, goal->request.name);
-
-  //  process_request.save_io = goal->request.save_io;
+  problem->env = env;
+  // process_request.save_io = goal->request.save_io;
+  problem->profiles = profiles_;
   problem->move_profile_remapping = tesseract_rosutils::fromMsg(goal->request.move_profile_remapping);
   problem->composite_profile_remapping = tesseract_rosutils::fromMsg(goal->request.composite_profile_remapping);