fixup

tesseract-robotics · Jul 12, 2024 · d18d15e · d18d15e
1 parent ddb2077
commit d18d15e
Show file tree

Hide file tree

Showing 22 changed files with 129 additions and 129 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
@@ -213,23 +213,23 @@ class TaskComposerNode
 
   /**
    * @brief A utility function for extracting data from data storage
-   * @param port The port associated with the key
    * @param data_storage The data storage to retrieve data from
+   * @param port The port associated with the key
    * @param required Indicate if data is required
    * @return The data stored under the name, if not found and required an exception will be thrown other null
    */
   template <typename T = tesseract_common::AnyPoly>
-  T getData(const std::string& port, const TaskComposerDataStorage& data_storage, bool required = true) const;
+  T getData(const TaskComposerDataStorage& data_storage, const std::string& port, bool required = true) const;
 
   /**
    * @brief A utility function for setting data in data storage
    * @param port The port associated with the key
    * @param data_storage The data storage to assign data to
    * @param data The data to store
    */
-  void setData(const std::string& port, TaskComposerDataStorage& data_storage, tesseract_common::AnyPoly data) const;
-  void setData(const std::string& port,
-               TaskComposerDataStorage& data_storage,
+  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;
 };
 

diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp
@@ -459,8 +459,8 @@ std::string TaskComposerNode::toString(const boost::uuids::uuid& u, const std::s
 }
 
 template <>
-tesseract_common::AnyPoly TaskComposerNode::getData(const std::string& port,
-                                                    const TaskComposerDataStorage& data_storage,
+tesseract_common::AnyPoly TaskComposerNode::getData(const TaskComposerDataStorage& data_storage,
+                                                    const std::string& port,
                                                     bool required) const
 {
   auto it = input_keys_.data().find(port);
@@ -481,8 +481,8 @@ tesseract_common::AnyPoly TaskComposerNode::getData(const std::string& port,
 }
 
 template <>
-std::vector<tesseract_common::AnyPoly> TaskComposerNode::getData(const std::string& port,
-                                                                 const TaskComposerDataStorage& data_storage,
+std::vector<tesseract_common::AnyPoly> TaskComposerNode::getData(const TaskComposerDataStorage& data_storage,
+                                                                 const std::string& port,
                                                                  bool required) const
 {
   auto it = input_keys_.data().find(port);
@@ -515,8 +515,8 @@ std::vector<tesseract_common::AnyPoly> TaskComposerNode::getData(const std::stri
   return data_container;
 }
 
-void TaskComposerNode::setData(const std::string& port,
-                               TaskComposerDataStorage& data_storage,
+void TaskComposerNode::setData(TaskComposerDataStorage& data_storage,
+                               const std::string& port,
                                tesseract_common::AnyPoly data) const
 {
   auto it = output_keys_.data().find(port);
@@ -527,8 +527,8 @@ void TaskComposerNode::setData(const std::string& port,
   data_storage.setData(key, std::move(data));
 }
 
-void TaskComposerNode::setData(const std::string& port,
-                               TaskComposerDataStorage& data_storage,
+void TaskComposerNode::setData(TaskComposerDataStorage& data_storage,
+                               const std::string& port,
                                const std::vector<tesseract_common::AnyPoly>& data) const
 {
   auto it = output_keys_.data().find(port);

diff --git a/tesseract_task_composer/examples/task_composer_example.cpp b/tesseract_task_composer/examples/task_composer_example.cpp
@@ -45,9 +45,9 @@ class AddTaskComposerNode : public TaskComposerTask
     auto info = std::make_unique<TaskComposerNodeInfo>(*this);
     info->return_value = 0;
     std::cout << name_ << std::endl;
-    double result = getData(INPUT_LEFT_PORT, *context.data_storage).as<double>() +
-                    getData(INPUT_RIGHT_PORT, *context.data_storage).as<double>();
-    setData(OUTPUT_RESULT_PORT, *context.data_storage, result);
+    double result = getData(*context.data_storage, INPUT_LEFT_PORT).as<double>() +
+                    getData(*context.data_storage, INPUT_RIGHT_PORT).as<double>();
+    setData(*context.data_storage, OUTPUT_RESULT_PORT, result);
     return info;
   }
 };
@@ -88,9 +88,9 @@ class MultiplyTaskComposerNode : public TaskComposerTask
     auto info = std::make_unique<TaskComposerNodeInfo>(*this);
     info->return_value = 0;
     std::cout << name_ << std::endl;
-    double result = getData(INPUT_LEFT_PORT, *context.data_storage).as<double>() *
-                    getData(INPUT_RIGHT_PORT, *context.data_storage).as<double>();
-    setData(OUTPUT_RESULT_PORT, *context.data_storage, result);
+    double result = getData(*context.data_storage, INPUT_LEFT_PORT).as<double>() *
+                    getData(*context.data_storage, INPUT_RIGHT_PORT).as<double>();
+    setData(*context.data_storage, OUTPUT_RESULT_PORT, result);
     return info;
   }
 };

diff --git a/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp
@@ -148,7 +148,7 @@ class MotionPlannerTask : public TaskComposerTask
     // --------------------
     // Check that inputs are valid
     // --------------------
-    auto env_poly = getData(INPUT_ENVIRONMENT_PORT, *context.data_storage);
+    auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
     if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
     {
       info->status_code = 0;
@@ -162,7 +162,7 @@ class MotionPlannerTask : public TaskComposerTask
         env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
     info->env = env;
 
-    auto input_data_poly = getData(INOUT_PROGRAM_PORT, *context.data_storage);
+    auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
     if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
     {
       info->status_message = "Input instructions to MotionPlannerTask: " + name_ + " must be a composite instruction";
@@ -171,13 +171,13 @@ class MotionPlannerTask : public TaskComposerTask
     }
     tesseract_common::AnyPoly original_input_data_poly{ input_data_poly };
 
-    auto profiles = getData(INPUT_PROFILES_PORT, *context.data_storage).as<std::shared_ptr<ProfileDictionary>>();
+    auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as<std::shared_ptr<ProfileDictionary>>();
     auto composite_profile_remapping_poly =
-        getData(INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, *context.data_storage, false);
-    auto move_profile_remapping_poly = getData(INPUT_MOVE_PROFILE_REMAPPING_PORT, *context.data_storage, false);
+        getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false);
+    auto move_profile_remapping_poly = getData(*context.data_storage, INPUT_MOVE_PROFILE_REMAPPING_PORT, false);
 
     tesseract_common::ManipulatorInfo input_manip_info;
-    auto manip_info_poly = getData(INPUT_MANIP_INFO_PORT, *context.data_storage, false);
+    auto manip_info_poly = getData(*context.data_storage, INPUT_MANIP_INFO_PORT, false);
     if (!manip_info_poly.isNull())
       input_manip_info = manip_info_poly.as<tesseract_common::ManipulatorInfo>();
 
@@ -207,7 +207,7 @@ class MotionPlannerTask : public TaskComposerTask
     if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG)
       request.verbose = true;
     PlannerResponse response = planner_->solve(request);
-    setData(INOUT_PROGRAM_PORT, *context.data_storage, response.results);
+    setData(*context.data_storage, INOUT_PROGRAM_PORT, response.results);
 
     // --------------------
     // Verify Success
@@ -230,7 +230,7 @@ class MotionPlannerTask : public TaskComposerTask
     // If the output key is not the same as the input key the output data should be assigned the input data for error
     // branching
     if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-      setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+      setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
     info->status_message = response.message;
     return info;

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
@@ -112,7 +112,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
   // --------------------
   // Check that inputs are valid
   // --------------------
-  auto env_poly = getData(INPUT_ENVIRONMENT_PORT, *context.data_storage);
+  auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
   if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
   {
     info->status_code = 0;
@@ -126,7 +126,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
       env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
   info->env = env;
 
-  auto input_data_poly = getData(INPUT_PROGRAM_PORT, *context.data_storage);
+  auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->status_code = 0;
@@ -137,13 +137,13 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
   }
 
   tesseract_common::ManipulatorInfo input_manip_info;
-  auto manip_info_poly = getData(INPUT_MANIP_INFO_PORT, *context.data_storage, false);
+  auto manip_info_poly = getData(*context.data_storage, INPUT_MANIP_INFO_PORT, false);
   if (!manip_info_poly.isNull())
     input_manip_info = manip_info_poly.as<tesseract_common::ManipulatorInfo>();
 
   // Get Composite Profile
-  auto profiles = getData(INPUT_PROFILES_PORT, *context.data_storage).as<std::shared_ptr<ProfileDictionary>>();
-  auto composite_profile_remapping_poly = getData(INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, *context.data_storage, false);
+  auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as<std::shared_ptr<ProfileDictionary>>();
+  auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false);
   const auto& ci = input_data_poly.as<CompositeInstruction>();
   std::string profile = ci.getProfile();
   profile = getProfileString(ns_, profile, composite_profile_remapping_poly);

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
@@ -112,7 +112,7 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
   // --------------------
   // Check that inputs are valid
   // --------------------
-  auto env_poly = getData(INPUT_ENVIRONMENT_PORT, *context.data_storage);
+  auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
   if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
   {
     info->status_code = 0;
@@ -126,7 +126,7 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
       env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
   info->env = env;
 
-  auto input_data_poly = getData(INPUT_PROGRAM_PORT, *context.data_storage);
+  auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->status_message = "Input to DiscreteContactCheckTask must be a composite instruction";
@@ -135,13 +135,13 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
   }
 
   tesseract_common::ManipulatorInfo input_manip_info;
-  auto manip_info_poly = getData(INPUT_MANIP_INFO_PORT, *context.data_storage, false);
+  auto manip_info_poly = getData(*context.data_storage, INPUT_MANIP_INFO_PORT, false);
   if (!manip_info_poly.isNull())
     input_manip_info = manip_info_poly.as<tesseract_common::ManipulatorInfo>();
 
   // Get Composite Profile
-  auto profiles = getData(INPUT_PROFILES_PORT, *context.data_storage).as<std::shared_ptr<ProfileDictionary>>();
-  auto composite_profile_remapping_poly = getData(INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, *context.data_storage, false);
+  auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as<std::shared_ptr<ProfileDictionary>>();
+  auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false);
   const auto& ci = input_data_poly.as<CompositeInstruction>();
   std::string profile = ci.getProfile();
   profile = getProfileString(ns_, profile, composite_profile_remapping_poly);

diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp
@@ -105,7 +105,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
   // --------------------
   // Check that inputs are valid
   // --------------------
-  tesseract_common::AnyPoly env_poly = getData(INPUT_ENVIRONMENT_PORT, *context.data_storage);
+  tesseract_common::AnyPoly env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
   if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
   {
     info->status_code = 0;
@@ -117,7 +117,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
   std::shared_ptr<const tesseract_environment::Environment> env =
       env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
 
-  auto input_data_poly = getData(INOUT_PROGRAM_PORT, *context.data_storage);
+  auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->status_message = "Input instruction to FixStateBounds must be a composite instruction";
@@ -128,12 +128,12 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
   tesseract_common::AnyPoly original_input_data_poly{ input_data_poly };
 
   tesseract_common::ManipulatorInfo input_manip_info;
-  auto manip_info_poly = getData(INPUT_MANIP_INFO_PORT, *context.data_storage, false);
+  auto manip_info_poly = getData(*context.data_storage, INPUT_MANIP_INFO_PORT, false);
   if (!manip_info_poly.isNull())
     input_manip_info = manip_info_poly.as<tesseract_common::ManipulatorInfo>();
 
-  auto profiles = getData(INPUT_PROFILES_PORT, *context.data_storage).as<std::shared_ptr<ProfileDictionary>>();
-  auto composite_profile_remapping_poly = getData(INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, *context.data_storage, false);
+  auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as<std::shared_ptr<ProfileDictionary>>();
+  auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false);
   auto& ci = input_data_poly.as<CompositeInstruction>();
   ci.setManipulatorInfo(ci.getManipulatorInfo().getCombined(input_manip_info));
   const tesseract_common::ManipulatorInfo& manip_info = ci.getManipulatorInfo();
@@ -167,7 +167,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
               // If the output key is not the same as the input key the output data should be assigned the input data
               // for error branching
               if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-                setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+                setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
               info->status_message = "Failed to clamp to joint limits";
               return info;
@@ -193,7 +193,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
               // If the output key is not the same as the input key the output data should be assigned the input data
               // for error branching
               if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-                setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+                setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
               info->status_message = "Failed to clamp to joint limits";
               return info;
@@ -211,7 +211,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
         // If the output key is not the same as the input key the output data should be assigned the input data for
         // error branching
         if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-          setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+          setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
         info->color = "green";
         info->status_code = 1;
@@ -242,7 +242,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
             // If the output key is not the same as the input key the output data should be assigned the input data for
             // error branching
             if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-              setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+              setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
             info->status_message = "Failed to clamp to joint limits";
             return info;
@@ -256,7 +256,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
       // If the output key is not the same as the input key the output data should be assigned the input data for
       // error branching
       if (output_keys_.get(INOUT_PROGRAM_PORT) != input_keys_.get(INOUT_PROGRAM_PORT))
-        setData(INOUT_PROGRAM_PORT, *context.data_storage, original_input_data_poly);
+        setData(*context.data_storage, INOUT_PROGRAM_PORT, original_input_data_poly);
 
       info->color = "green";
       info->status_code = 1;
@@ -266,7 +266,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
     }
   }
 
-  setData(INOUT_PROGRAM_PORT, *context.data_storage, input_data_poly);
+  setData(*context.data_storage, INOUT_PROGRAM_PORT, input_data_poly);
 
   info->color = "green";
   info->status_code = 1;