Merge branch 'master' into update/ci

tesseract_examples/src/basic_cartesian_example.cpp

@@ -244,7 +244,7 @@ bool BasicCartesianExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   if (plotter_ != nullptr && plotter_->isConnected())
     plotter_->waitForInput("Hit Enter to solve for trajectory.");

tesseract_examples/src/car_seat_example.cpp

@@ -302,7 +302,7 @@ bool CarSeatExample::run()
 
     // Create Task Composer Problem
     auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-    problem->input_instruction = program;
+    problem->input = program;
 
     // Solve task
     TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
@@ -386,7 +386,7 @@ bool CarSeatExample::run()
 
     // Create Task Composer Problem
     auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-    problem->input_instruction = program;
+    problem->input = program;
 
     // Solve task
     TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));

tesseract_examples/src/freespace_hybrid_example.cpp

@@ -180,7 +180,7 @@ bool FreespaceHybridExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   // Solve task
   TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));

tesseract_examples/src/freespace_ompl_example.cpp

@@ -180,7 +180,7 @@ bool FreespaceOMPLExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   // Solve task
   TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));

tesseract_examples/src/glass_upright_example.cpp

@@ -237,7 +237,7 @@ bool GlassUprightExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   if (plotter_ != nullptr && plotter_->isConnected())
     plotter_->waitForInput("Hit Enter to solve for trajectory.");

tesseract_examples/src/pick_and_place_example.cpp

@@ -227,7 +227,7 @@ bool PickAndPlaceExample::run()
 
   // Create Task Composer Problem
   auto pick_problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  pick_problem->input_instruction = pick_program;
+  pick_problem->input = pick_program;
 
   // Solve task
   TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem));
@@ -344,7 +344,7 @@ bool PickAndPlaceExample::run()
 
   // Create Task Composer Problem
   auto place_problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  place_problem->input_instruction = place_program;
+  place_problem->input = place_program;
 
   // Solve task
   TaskComposerFuture::UPtr place_future = executor->run(*place_task, std::move(place_problem));

tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp

@@ -233,7 +233,7 @@ bool PuzzlePieceAuxillaryAxesExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   // Solve task
   TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));

tesseract_examples/src/puzzle_piece_example.cpp

@@ -225,7 +225,7 @@ bool PuzzlePieceExample::run()
 
   // Create Task Composer Problem
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
-  problem->input_instruction = program;
+  problem->input = program;
 
   // Solve task
   tesseract_common::Timer stopwatch;

tesseract_motion_planners/core/src/utils.cpp

@@ -317,8 +317,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
   {
     // Grab the first waypoint to get the joint names
     const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
-    traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names,
-                                                                                    static_cast<int>(program.size()));
+    traj_contacts =
+        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(program.size()));
   }
 
   contacts.clear();
@@ -624,8 +624,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
   {
     // Grab the first waypoint to get the joint names
     const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
-    traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names,
-                                                                                    static_cast<int>(program.size()));
+    traj_contacts =
+        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(program.size()));
   }
 
   contacts.clear();

tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h

@@ -81,9 +81,6 @@ class TaskComposerNodeInfo
   /** @brief The output keys */
   std::vector<std::string> output_keys;
 
-  /** @brief Store the results of the task */
-  tesseract_common::AnyPoly results;
-
   /** @brief Value returned from the Task on completion */
   int return_value{ -1 };
 

tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h

@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/serialization.h>
+#include <tesseract_common/any_poly.h>
 #include <tesseract_task_composer/core/task_composer_data_storage.h>
 
 namespace tesseract_planning
@@ -58,6 +59,9 @@ struct TaskComposerProblem
   /** @brief Indicate if dotgraph should be provided */
   bool dotgraph{ false };
 
+  /** @brief The problem input */
+  tesseract_common::AnyPoly input;
+
   /**
    * @brief Clone the planning problem
    * @return A clone of the planning problem

tesseract_task_composer/core/src/task_composer_node_info.cpp

@@ -58,7 +58,6 @@ bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const
   equal &= name == rhs.name;
   equal &= uuid == rhs.uuid;
   equal &= parent_uuid == rhs.parent_uuid;
-  equal &= results == rhs.results;
   equal &= return_value == rhs.return_value;
   equal &= message == rhs.message;
   equal &= tesseract_common::almostEqualRelativeAndAbs(elapsed_time, rhs.elapsed_time, max_diff);
@@ -84,7 +83,6 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/
   ar& boost::serialization::make_nvp("name", name);
   ar& boost::serialization::make_nvp("uuid", uuid);
   ar& boost::serialization::make_nvp("parent_uuid", parent_uuid);
-  ar& boost::serialization::make_nvp("results", results);
   ar& boost::serialization::make_nvp("return_value", return_value);
   ar& boost::serialization::make_nvp("message", message);
   ar& boost::serialization::make_nvp("elapsed_time", elapsed_time);

tesseract_task_composer/core/src/task_composer_problem.cpp

@@ -46,6 +46,7 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const
   bool equal = true;
   equal &= name == rhs.name;
   equal &= dotgraph == rhs.dotgraph;
+  equal &= input == input;
   return equal;
 }
 
@@ -56,6 +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);
 }
 
 }  // namespace tesseract_planning

tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h

@@ -86,9 +86,6 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem
   /** @brief The Profiles to use */
   ProfileDictionary::ConstPtr profiles;
 
-  /** @brief The problem input instruction */
-  InstructionPoly input_instruction;
-
   /**
    * @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a
    * given motion planner.

tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp

@@ -66,18 +66,18 @@ TaskComposerNodeInfo::UPtr ProcessPlanningInputTask::runImpl(TaskComposerContext
 
   auto info = std::make_unique<TaskComposerNodeInfo>(*this);
 
-  if (!problem.input_instruction.isCompositeInstruction())
+  if (problem.input.isNull() || problem.input.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->color = "red";
-    info->message = "Input instructon is not a Composite Instruction, aborting...";
+    info->message = "Input is not a Composite Instruction, aborting...";
     info->return_value = 0;
 
     // Abort
     context.abort(uuid_);
     return info;
   }
 
-  context.data_storage->setData(output_keys_[0], problem.input_instruction.as<CompositeInstruction>());
+  context.data_storage->setData(output_keys_[0], problem.input.as<CompositeInstruction>());
 
   info->color = "green";
   info->message = "Successful";

tesseract_task_composer/planning/src/planning_task_composer_problem.cpp

@@ -102,7 +102,6 @@ bool PlanningTaskComposerProblem::operator==(const PlanningTaskComposerProblem&
   equal &= tesseract_common::pointersEqual(env, rhs.env);
   equal &= manip_info == rhs.manip_info;
   //  equal &= tesseract_common::pointersEqual(profiles, rhs.profiles);
-  equal &= input_instruction == rhs.input_instruction;
   equal &= move_profile_remapping == rhs.move_profile_remapping;
   equal &= composite_profile_remapping == rhs.composite_profile_remapping;
   return equal;
@@ -118,7 +117,6 @@ void PlanningTaskComposerProblem::serialize(Archive& ar, const unsigned int /*ve
   ar& boost::serialization::make_nvp("manip_info", manip_info);
   /** @todo Fix after profiles are serializable */
   //  ar& boost::serialization::make_nvp("profiles", profiles);
-  ar& boost::serialization::make_nvp("input_instruction", input_instruction);
   ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping);
   ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping);
 }

tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp

@@ -91,9 +91,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests)  //
     EXPECT_TRUE(problem.move_profile_remapping.empty());
     EXPECT_TRUE(problem.composite_profile_remapping.empty());
     EXPECT_TRUE(problem.profiles != nullptr);
-    EXPECT_TRUE(problem.input_instruction.isNull());
-    problem.input_instruction = test_suite::freespaceExampleProgramABB();
-    EXPECT_FALSE(problem.input_instruction.isNull());
+    EXPECT_TRUE(problem.input.isNull());
+    problem.input = test_suite::freespaceExampleProgramABB();
+    EXPECT_FALSE(problem.input.isNull());
 
     // Test clone
     auto clone = problem.clone();
@@ -110,9 +110,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests)  //
     EXPECT_TRUE(problem.move_profile_remapping.empty());
     EXPECT_TRUE(problem.composite_profile_remapping.empty());
     EXPECT_TRUE(problem.profiles != nullptr);
-    EXPECT_TRUE(problem.input_instruction.isNull());
-    problem.input_instruction = test_suite::freespaceExampleProgramABB();
-    EXPECT_FALSE(problem.input_instruction.isNull());
+    EXPECT_TRUE(problem.input.isNull());
+    problem.input = test_suite::freespaceExampleProgramABB();
+    EXPECT_FALSE(problem.input.isNull());
 
     // Test clone
     auto clone = problem.clone();
@@ -129,9 +129,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests)  //
     EXPECT_TRUE(problem.move_profile_remapping.empty());
     EXPECT_TRUE(problem.composite_profile_remapping.empty());
     EXPECT_TRUE(problem.profiles != nullptr);
-    EXPECT_TRUE(problem.input_instruction.isNull());
-    problem.input_instruction = test_suite::freespaceExampleProgramABB();
-    EXPECT_FALSE(problem.input_instruction.isNull());
+    EXPECT_TRUE(problem.input.isNull());
+    problem.input = test_suite::freespaceExampleProgramABB();
+    EXPECT_FALSE(problem.input.isNull());
 
     // Test clone
     auto clone = problem.clone();
@@ -148,9 +148,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests)  //
     EXPECT_TRUE(problem.move_profile_remapping.empty());
     EXPECT_TRUE(problem.composite_profile_remapping.empty());
     EXPECT_TRUE(problem.profiles != nullptr);
-    EXPECT_TRUE(problem.input_instruction.isNull());
-    problem.input_instruction = test_suite::freespaceExampleProgramABB();
-    EXPECT_FALSE(problem.input_instruction.isNull());
+    EXPECT_TRUE(problem.input.isNull());
+    problem.input = test_suite::freespaceExampleProgramABB();
+    EXPECT_FALSE(problem.input.isNull());
 
     // Test clone
     auto clone = problem.clone();
@@ -2554,7 +2554,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests)  //
     // Create problem
     auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
     problem->dotgraph = true;
-    problem->input_instruction = test_suite::rasterExampleProgram();
+    problem->input = test_suite::rasterExampleProgram();
 
     // Solve raster plan
     auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
@@ -3025,7 +3025,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests)
     // Create problem
     auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
     problem->dotgraph = true;
-    problem->input_instruction = test_suite::rasterOnlyExampleProgram();
+    problem->input = test_suite::rasterOnlyExampleProgram();
 
     // Solve raster plan
     auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");