diff --git a/tesseract_msgs/CMakeLists.txt b/tesseract_msgs/CMakeLists.txt index 444916fb..03d1f270 100644 --- a/tesseract_msgs/CMakeLists.txt +++ b/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" diff --git a/tesseract_msgs/msg/TaskComposerKey.msg b/tesseract_msgs/msg/TaskComposerKey.msg new file mode 100644 index 00000000..0606edb1 --- /dev/null +++ b/tesseract_msgs/msg/TaskComposerKey.msg @@ -0,0 +1,3 @@ +string port +string[] keys +uint8 type_index \ No newline at end of file diff --git a/tesseract_msgs/msg/TaskComposerNodeInfo.msg b/tesseract_msgs/msg/TaskComposerNodeInfo.msg index 9865a314..43b42cee 100644 --- a/tesseract_msgs/msg/TaskComposerNodeInfo.msg +++ b/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 diff --git a/tesseract_planning_server/CMakeLists.txt b/tesseract_planning_server/CMakeLists.txt index 5b1505ff..9436c31e 100644 --- a/tesseract_planning_server/CMakeLists.txt +++ b/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} diff --git a/tesseract_planning_server/src/tesseract_planning_server.cpp b/tesseract_planning_server/src/tesseract_planning_server.cpp index 665f3423..ae408470 100644 --- a/tesseract_planning_server/src/tesseract_planning_server.cpp +++ b/tesseract_planning_server/src/tesseract_planning_server.cpp @@ -57,8 +57,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #endif -#include - #include #include #include @@ -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(goal->request.name); + tesseract_common::AnyPoly planning_input; try { - problem->input = Serialization::fromArchiveStringXML(goal->request.input); + planning_input = Serialization::fromArchiveStringXML(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(); + 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(), 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( results.as()); } diff --git a/tesseract_rosutils/include/tesseract_rosutils/utils.h b/tesseract_rosutils/include/tesseract_rosutils/utils.h index ff5e4f8f..53e352e3 100644 --- a/tesseract_rosutils/include/tesseract_rosutils/utils.h +++ b/tesseract_rosutils/include/tesseract_rosutils/utils.h @@ -59,6 +59,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include #include #include diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp index 5d4ab2e7..0334e5b1 100644 --- a/tesseract_rosutils/src/utils.cpp +++ b/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(pair.second)); + } + else + { + for (const auto& key : std::get>(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(pair.second)); + } + else + { + key_msg.type_index = 1; + for (const auto& key : std::get>(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(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;