Add optional output to task which produce contact results

tesseract-robotics · Aug 18, 2024 · 050efde · 050efde
1 parent d8d1e0a
commit 050efde
Show file tree

Hide file tree

Showing 8 changed files with 45 additions and 6 deletions.
diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h
@@ -240,11 +240,16 @@ class TaskComposerNode
    * @param port The port associated with the key
    * @param data_storage The data storage to assign data to
    * @param data The data to store
+   * @param required Indicate if required port
    */
-  void setData(TaskComposerDataStorage& data_storage, const std::string& port, tesseract_common::AnyPoly data) const;
   void setData(TaskComposerDataStorage& data_storage,
                const std::string& port,
-               const std::vector<tesseract_common::AnyPoly>& data) const;
+               tesseract_common::AnyPoly data,
+               bool required = true) const;
+  void setData(TaskComposerDataStorage& data_storage,
+               const std::string& port,
+               const std::vector<tesseract_common::AnyPoly>& data,
+               bool required = true) const;
 };
 
 }  // namespace tesseract_planning

diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp
@@ -577,23 +577,35 @@ std::vector<tesseract_common::AnyPoly> TaskComposerNode::getData(const TaskCompo
 
 void TaskComposerNode::setData(TaskComposerDataStorage& data_storage,
                                const std::string& port,
-                               tesseract_common::AnyPoly data) const
+                               tesseract_common::AnyPoly data,
+                               bool required) const
 {
   auto it = output_keys_.data().find(port);
   if (it == output_keys_.data().end())
-    throw std::runtime_error(name_ + ", output key does not exist for the provided name: " + port);
+  {
+    if (required)
+      throw std::runtime_error(name_ + ", output key does not exist for the provided name: " + port);
+
+    return;
+  }
 
   const auto& key = std::get<std::string>(it->second);
   data_storage.setData(key, std::move(data));
 }
 
 void TaskComposerNode::setData(TaskComposerDataStorage& data_storage,
                                const std::string& port,
-                               const std::vector<tesseract_common::AnyPoly>& data) const
+                               const std::vector<tesseract_common::AnyPoly>& data,
+                               bool required) const
 {
   auto it = output_keys_.data().find(port);
   if (it == output_keys_.data().end())
-    throw std::runtime_error(name_ + ", output key does not exist for the provided name: " + port);
+  {
+    if (required)
+      throw std::runtime_error(name_ + ", output key does not exist for the provided name: " + port);
+
+    return;
+  }
 
   const auto& vs = std::get<std::vector<std::string>>(it->second);
   if (vs.size() != data.size())

diff --git a/...r/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h b/...r/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h
@@ -53,6 +53,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT ContinuousContactCheckTask :
   // Optional
   static const std::string INPUT_MANIP_INFO_PORT;
   static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT;
+  static const std::string OUTPUT_CONTACT_RESULTS_PORT;
 
   using Ptr = std::shared_ptr<ContinuousContactCheckTask>;
   using ConstPtr = std::shared_ptr<const ContinuousContactCheckTask>;

diff --git a/...ser/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h b/...ser/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h
@@ -53,6 +53,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT DiscreteContactCheckTask : p
   // Optional
   static const std::string INPUT_MANIP_INFO_PORT;
   static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT;
+  static const std::string OUTPUT_CONTACT_RESULTS_PORT;
 
   using Ptr = std::shared_ptr<DiscreteContactCheckTask>;
   using ConstPtr = std::shared_ptr<const DiscreteContactCheckTask>;

diff --git a/...mposer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h b/...mposer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h
@@ -62,6 +62,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT FixStateCollisionTask : publ
   // Optional
   static const std::string INPUT_MANIP_INFO_PORT;
   static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT;
+  static const std::string OUTPUT_CONTACT_RESULTS_PORT;
 
   using Ptr = std::shared_ptr<FixStateCollisionTask>;
   using ConstPtr = std::shared_ptr<const FixStateCollisionTask>;

diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp
@@ -64,6 +64,7 @@ const std::string ContinuousContactCheckTask::INPUT_PROFILES_PORT = "profiles";
 // Optional
 const std::string ContinuousContactCheckTask::INPUT_MANIP_INFO_PORT = "manip_info";
 const std::string ContinuousContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping";
+const std::string ContinuousContactCheckTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results";
 
 ContinuousContactCheckTask::ContinuousContactCheckTask()
   : TaskComposerTask("ContinuousContactCheckTask", ContinuousContactCheckTask::ports(), true)
@@ -99,6 +100,8 @@ TaskComposerNodePorts ContinuousContactCheckTask::ports()
 
   ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE;
   ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE;
+  ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE;
+
   return ports;
 }
 
@@ -171,6 +174,8 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
       contact_map.shrinkToFit();
 
     info->data_storage.setData("contact_results", contacts);
+    setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contacts, false);
+
     info->return_value = 0;
     return info;
   }

diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp
@@ -64,6 +64,7 @@ const std::string DiscreteContactCheckTask::INPUT_PROFILES_PORT = "profiles";
 // Optional
 const std::string DiscreteContactCheckTask::INPUT_MANIP_INFO_PORT = "manip_info";
 const std::string DiscreteContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping";
+const std::string DiscreteContactCheckTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results";
 
 DiscreteContactCheckTask::DiscreteContactCheckTask()
   : TaskComposerTask("DiscreteContactCheckTask", DiscreteContactCheckTask::ports(), true)
@@ -99,6 +100,8 @@ TaskComposerNodePorts DiscreteContactCheckTask::ports()
 
   ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE;
   ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE;
+
+  ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE;
   return ports;
 }
 
@@ -167,6 +170,8 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
       contact_map.shrinkToFit();
 
     info->data_storage.setData("contact_results", contacts);
+    setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contacts, false);
+
     return info;
   }
 

diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp
@@ -67,6 +67,7 @@ const std::string FixStateCollisionTask::INPUT_PROFILES_PORT = "profiles";
 // Optional
 const std::string FixStateCollisionTask::INPUT_MANIP_INFO_PORT = "manip_info";
 const std::string FixStateCollisionTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping";
+const std::string FixStateCollisionTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results";
 
 bool stateInCollision(const Eigen::Ref<const Eigen::VectorXd>& start_pos,
                       const tesseract_common::ManipulatorInfo& manip_info,
@@ -384,6 +385,7 @@ TaskComposerNodePorts FixStateCollisionTask::ports()
   ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE;
 
   ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE;
+  ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE;
 
   return ports;
 }
@@ -461,6 +463,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
             return info;
           }
         }
@@ -491,6 +494,8 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
+
             return info;
           }
         }
@@ -558,6 +563,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
             return info;
           }
         }
@@ -613,6 +619,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
             return info;
           }
         }
@@ -670,6 +677,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
             return info;
           }
         }
@@ -725,6 +733,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
 
             info->status_message = "Failed to correct state in collision";
             info->data_storage.setData("contact_results", contact_results);
+            setData(*context.data_storage, OUTPUT_CONTACT_RESULTS_PORT, contact_results, false);
             return info;
           }
         }