Update because TaskComposerProblem was removed

tesseract_msgs/CMakeLists.txt

@@ -76,6 +76,7 @@ set(message_files
   "msg/StringLimitsPair.msg"
   "msg/StringPair.msg"
   "msg/StringPluginInfoPair.msg"
+  "msg/TaskComposerKey.msg"
   "msg/TaskComposerNodeInfo.msg"
   "msg/Trajectory.msg"
   "msg/TransformMap.msg"

tesseract_msgs/msg/TaskComposerKey.msg

@@ -0,0 +1,3 @@
+string port
+string[] keys
+uint8 type_index

tesseract_msgs/msg/TaskComposerNodeInfo.msg

@@ -13,10 +13,10 @@ string[] inbound_edges
 string[] outbound_edges
 
 # Input keys
-string[] input_keys
+TaskComposerKey[] input_keys
 
 # Output keys
-string[] output_keys
+TaskComposerKey[] output_keys
 
 # Value returned from the Task on completion
 int32 return_value

tesseract_planning_server/CMakeLists.txt

@@ -39,7 +39,6 @@ target_link_libraries(${PROJECT_NAME}
     tesseract::tesseract_motion_planners_ompl
     tesseract::tesseract_motion_planners_descartes
     tesseract::tesseract_task_composer
-    tesseract::tesseract_task_composer_planning
     tesseract::tesseract_task_composer_planning_nodes
 
     ${tesseract_msgs_TARGETS}

tesseract_planning_server/src/tesseract_planning_server.cpp

@@ -57,8 +57,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
 #endif
 
-#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
-
 #include <tesseract_environment/environment.h>
 #include <tesseract_environment/environment_cache.h>
 #include <tesseract_command_language/profile_dictionary.h>
@@ -80,7 +78,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 using tesseract_common::Serialization;
 using tesseract_planning::InstructionPoly;
-using tesseract_planning::PlanningTaskComposerProblem;
 using tesseract_planning::TaskComposerDataStorage;
 
 static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask";
@@ -221,17 +218,17 @@ void TesseractPlanningServer::onMotionPlanningCallback(
     return;
   }
 
-  auto problem = std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(goal->request.name);
+  tesseract_common::AnyPoly planning_input;
   try
   {
-    problem->input = Serialization::fromArchiveStringXML<tesseract_common::AnyPoly>(goal->request.input);
+    planning_input = Serialization::fromArchiveStringXML<tesseract_common::AnyPoly>(goal->request.input);
   }
   catch (const std::exception& e)
   {
     result->response.successful = false;
     std::ostringstream oss;
-    oss << "Failed to deserialize program instruction with error: '" << e.what() << "'!" << std::endl;
-    oss << " Make sure the program was serialized from an InstructionPoly type." << std::endl;
+    oss << "Failed to deserialize planning input instruction with error: '" << e.what() << "'!" << std::endl;
+    oss << " Make sure the program was serialized from an AnyPoly type." << std::endl;
     RCLCPP_ERROR_STREAM(node_->get_logger(), oss.str());
     goal_handle->succeed(result);
     return;
@@ -245,12 +242,18 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   env->applyCommands(tesseract_rosutils::fromMsg(goal->request.commands));
   env->setState(env_state.joints);
 
-  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);
-  problem->dotgraph = goal->request.dotgraph;
+  // Create solve data storage
+  auto data = std::make_unique<TaskComposerDataStorage>();
+  data->setData("planning_input", std::move(planning_input));
+  data->setData("environment", std::move(env));
+  data->setData("profiles", profiles_);
+  auto move_profile_remapping = tesseract_rosutils::fromMsg(goal->request.move_profile_remapping);
+  if (!move_profile_remapping.empty())
+    data->setData("move_profile_remapping", move_profile_remapping);
+
+  auto composite_profile_remapping = tesseract_rosutils::fromMsg(goal->request.composite_profile_remapping);
+  if (!composite_profile_remapping.empty())
+    data->setData("composite_profile_remapping", composite_profile_remapping);
 
   // Store the initial state in the response for publishing trajectories
   tesseract_scene_graph::SceneState initial_state = env->getState();
@@ -260,7 +263,7 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   tesseract_common::Timer timer;
   timer.start();
   tesseract_planning::TaskComposerFuture::UPtr plan_future =
-      planning_server_->run(std::move(problem), std::make_unique<TaskComposerDataStorage>(), executor_name);
+      planning_server_->run(goal->request.name, std::move(data), goal->request.dotgraph, executor_name);
   plan_future->wait();  // Wait for results
   timer.stop();
 
@@ -270,7 +273,7 @@ void TesseractPlanningServer::onMotionPlanningCallback(
     try
     {
       // Save dot graph
-      const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
+      const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->name);
       std::stringstream dotgraph;
       task.dump(dotgraph, nullptr, plan_future->context->task_infos.getInfoMap());
       result->response.dotgraph = dotgraph.str();
@@ -285,8 +288,9 @@ void TesseractPlanningServer::onMotionPlanningCallback(
 
   try
   {
-    const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
-    tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(task.getOutputKeys().front());
+    const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->name);
+    tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(task.getOutputKeys().get("proble"
+                                                                                                             "m"));
     result->response.results = Serialization::toArchiveStringXML<tesseract_planning::InstructionPoly>(
         results.as<tesseract_planning::CompositeInstruction>());
   }

tesseract_rosutils/include/tesseract_rosutils/utils.h

@@ -59,6 +59,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_msgs/msg/transform_map.hpp>
 #include <tesseract_msgs/msg/visual_geometry.hpp>
 #include <tesseract_msgs/msg/planner_profile_remapping.hpp>
+#include <tesseract_msgs/msg/task_composer_key.hpp>
 #include <tesseract_msgs/msg/task_composer_node_info.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>

tesseract_rosutils/src/utils.cpp

@@ -2342,8 +2342,41 @@ bool toMsg(tesseract_msgs::msg::TaskComposerNodeInfo& node_info_msg,
   node_info_msg.outbound_edges.reserve(node_info.outbound_edges.size());
   for (const auto& edge : node_info.outbound_edges)
     node_info_msg.outbound_edges.push_back(boost::uuids::to_string(edge));
-  node_info_msg.input_keys = node_info.input_keys;
-  node_info_msg.output_keys = node_info.output_keys;
+
+  for (const auto& pair : node_info.input_keys.data())
+  {
+    tesseract_msgs::msg::TaskComposerKey key_msg;
+    key_msg.port = pair.first;
+    if (pair.second.index() == 0)
+    {
+      key_msg.keys.push_back(std::get<std::string>(pair.second));
+    }
+    else
+    {
+      for (const auto& key : std::get<std::vector<std::string>>(pair.second))
+        key_msg.keys.push_back(key);
+    }
+    node_info_msg.input_keys.push_back(key_msg);
+  }
+
+  for (const auto& pair : node_info.output_keys.data())
+  {
+    tesseract_msgs::msg::TaskComposerKey key_msg;
+    key_msg.port = pair.first;
+    if (pair.second.index() == 0)
+    {
+      key_msg.type_index = 0;
+      key_msg.keys.push_back(std::get<std::string>(pair.second));
+    }
+    else
+    {
+      key_msg.type_index = 1;
+      for (const auto& key : std::get<std::vector<std::string>>(pair.second))
+        key_msg.keys.push_back(key);
+    }
+    node_info_msg.output_keys.push_back(key_msg);
+  }
+
   node_info_msg.return_value = node_info.return_value;
   node_info_msg.status_code = node_info_msg.status_code;
   node_info_msg.status_message = node_info_msg.status_message;
@@ -2364,8 +2397,23 @@ tesseract_planning::TaskComposerNodeInfo::Ptr fromMsg(const tesseract_msgs::msg:
   node_info->outbound_edges.reserve(node_info_msg.outbound_edges.size());
   for (const auto& edge : node_info_msg.outbound_edges)
     node_info->outbound_edges.push_back(boost::lexical_cast<boost::uuids::uuid>(edge));
-  node_info->input_keys = node_info_msg.input_keys;
-  node_info->output_keys = node_info_msg.output_keys;
+
+  for (const auto& key_msg : node_info_msg.input_keys)
+  {
+    if (key_msg.type_index == 0)
+      node_info->input_keys.add(key_msg.port, key_msg.keys.front());
+    else
+      node_info->input_keys.add(key_msg.port, key_msg.keys);
+  }
+
+  for (const auto& key_msg : node_info_msg.output_keys)
+  {
+    if (key_msg.type_index == 0)
+      node_info->output_keys.add(key_msg.port, key_msg.keys.front());
+    else
+      node_info->output_keys.add(key_msg.port, key_msg.keys);
+  }
+
   node_info->return_value = node_info_msg.return_value;
   node_info->status_code = node_info_msg.status_code;
   node_info->status_message = node_info_msg.status_message;