Move task composer elapse timing to base classes

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

@@ -90,7 +90,10 @@ class TaskComposerNodeInfo
   /** @brief Status message */
   std::string message;
 
-  /** @brief Time spent in this task in seconds*/
+  /**
+   * @brief Time spent in this task in seconds
+   * @details This is managed by core components so implementation do not need to calculate this
+   */
   double elapsed_time{ 0 };
 
   /** @brief The DOT Graph color to fill with */

tesseract_task_composer/core/src/nodes/abort_task.cpp

@@ -27,7 +27,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/nodes/abort_task.h>
 

tesseract_task_composer/core/src/nodes/done_task.cpp

@@ -28,7 +28,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/nodes/done_task.h>
 

tesseract_task_composer/core/src/nodes/error_task.cpp

@@ -28,7 +28,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/nodes/error_task.h>
 

tesseract_task_composer/core/src/nodes/start_task.cpp

@@ -28,7 +28,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/nodes/start_task.h>
 

tesseract_task_composer/core/src/task_composer_pipeline.cpp

@@ -28,6 +28,7 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
+#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/task_composer_pipeline.h>
 #include <tesseract_task_composer/core/task_composer_node_info.h>
@@ -60,7 +61,9 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec
     return 0;
   }
 
+  tesseract_common::Timer timer;
   TaskComposerNodeInfo::UPtr results;
+  timer.start();
   try
   {
     results = runImpl(input, executor);
@@ -72,6 +75,8 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec
     results->message = "Exception thrown: " + std::string(e.what());
     results->return_value = 0;
   }
+  timer.stop();
+  results->elapsed_time = timer.elapsedSeconds();
 
   int value = results->return_value;
   assert(value >= 0);

tesseract_task_composer/core/src/task_composer_task.cpp

@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_task_composer/core/task_composer_task.h>
+#include <tesseract_common/timer.h>
 
 namespace tesseract_planning
 {
@@ -58,7 +59,9 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor
     return 0;
   }
 
+  tesseract_common::Timer timer;
   TaskComposerNodeInfo::UPtr results;
+  timer.start();
   try
   {
     results = runImpl(input, executor);
@@ -70,6 +73,8 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor
     results->message = "Exception thrown: " + std::string(e.what());
     results->return_value = 0;
   }
+  timer.stop();
+  results->elapsed_time = timer.elapsedSeconds();
 
   int value = results->return_value;
   assert(value >= 0);

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp

@@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/access.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/core/task_composer_task.h>
 #include <tesseract_task_composer/planning/nodes/motion_planner_task_info.h>
@@ -116,15 +115,12 @@ class MotionPlannerTask : public TaskComposerTask
   TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input,
                                      OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override
   {
-    auto info = std::make_unique<MotionPlannerTaskInfo>(*this);
-
     // Get the problem
     auto& problem = dynamic_cast<PlanningTaskComposerProblem&>(*input.problem);
 
+    auto info = std::make_unique<MotionPlannerTaskInfo>(*this);
     info->return_value = 0;
     info->env = problem.env;
-    tesseract_common::Timer timer;
-    timer.start();
 
     // --------------------
     // Check that inputs are valid
@@ -133,7 +129,6 @@ class MotionPlannerTask : public TaskComposerTask
     if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
     {
       info->message = "Input instructions to MotionPlannerTask: " + name_ + " must be a composite instruction";
-      info->elapsed_time = timer.elapsedSeconds();
       CONSOLE_BRIDGE_logError("%s", info->message.c_str());
       return info;
     }
@@ -173,7 +168,6 @@ class MotionPlannerTask : public TaskComposerTask
       info->return_value = 1;
       info->color = "green";
       info->message = response.message;
-      info->elapsed_time = timer.elapsedSeconds();
       CONSOLE_BRIDGE_logDebug("Motion Planner process succeeded");
       return info;
     }
@@ -189,7 +183,6 @@ class MotionPlannerTask : public TaskComposerTask
       input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0]));
 
     info->message = response.message;
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }
 };

tesseract_task_composer/planning/src/nodes/check_input_task.cpp

@@ -30,7 +30,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/serialization/vector.hpp>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 #include <tesseract_task_composer/planning/nodes/check_input_task.h>
 #include <tesseract_task_composer/planning/profiles/check_input_profile.h>
@@ -101,12 +100,7 @@ TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input,
   return info;
 }
 
-bool CheckInputTask::operator==(const CheckInputTask& rhs) const
-{
-  bool equal = true;
-  equal &= TaskComposerTask::operator==(rhs);
-  return equal;
-}
+bool CheckInputTask::operator==(const CheckInputTask& rhs) const { return (TaskComposerTask::operator==(rhs)); }
 bool CheckInputTask::operator!=(const CheckInputTask& rhs) const { return !operator==(rhs); }
 
 template <class Archive>

tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp

@@ -32,7 +32,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/serialization/shared_ptr.hpp>
 #include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 //#include <tesseract_process_managers/core/utils.h>
 #include <tesseract_task_composer/planning/nodes/continuous_contact_check_task.h>
@@ -70,15 +69,12 @@ ContinuousContactCheckTask::ContinuousContactCheckTask(std::string name,
 TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput& input,
                                                                OptionalTaskComposerExecutor /*executor*/) const
 {
-  auto info = std::make_unique<ContinuousContactCheckTaskInfo>(*this);
-
   // Get the problem
   auto& problem = dynamic_cast<PlanningTaskComposerProblem&>(*input.problem);
 
+  auto info = std::make_unique<ContinuousContactCheckTaskInfo>(*this);
   info->return_value = 0;
   info->env = problem.env;
-  tesseract_common::Timer timer;
-  timer.start();
 
   // --------------------
   // Check that inputs are valid
@@ -89,7 +85,6 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput
     info->message = "Input seed to ContinuousContactCheckTask must be a composite instruction";
     CONSOLE_BRIDGE_logError("%s", info->message.c_str());
     info->return_value = 0;
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }
 
@@ -123,23 +118,19 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput
 
     info->contact_results = contacts;
     info->return_value = 0;
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }
 
   info->color = "green";
   info->message = "Continuous contact check succeeded";
   CONSOLE_BRIDGE_logDebug("%s", info->message.c_str());
   info->return_value = 1;
-  info->elapsed_time = timer.elapsedSeconds();
   return info;
 }
 
 bool ContinuousContactCheckTask::operator==(const ContinuousContactCheckTask& rhs) const
 {
-  bool equal = true;
-  equal &= TaskComposerTask::operator==(rhs);
-  return equal;
+  return (TaskComposerTask::operator==(rhs));
 }
 bool ContinuousContactCheckTask::operator!=(const ContinuousContactCheckTask& rhs) const { return !operator==(rhs); }
 

tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp

@@ -32,7 +32,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/serialization/map.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 //#include <tesseract_process_managers/core/utils.h>
 #include <tesseract_task_composer/planning/nodes/discrete_contact_check_task.h>
@@ -70,15 +69,12 @@ DiscreteContactCheckTask::DiscreteContactCheckTask(std::string name,
 TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& input,
                                                              OptionalTaskComposerExecutor /*executor*/) const
 {
-  auto info = std::make_unique<DiscreteContactCheckTaskInfo>(*this);
-
   // Get the problem
   auto& problem = dynamic_cast<PlanningTaskComposerProblem&>(*input.problem);
 
+  auto info = std::make_unique<DiscreteContactCheckTaskInfo>(*this);
   info->return_value = 0;
   info->env = problem.env;
-  tesseract_common::Timer timer;
-  timer.start();
 
   // --------------------
   // Check that inputs are valid
@@ -87,7 +83,6 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput&
   if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->message = "Input to DiscreteContactCheckTask must be a composite instruction";
-    info->elapsed_time = timer.elapsedSeconds();
     CONSOLE_BRIDGE_logError("%s", info->message.c_str());
     return info;
   }
@@ -120,23 +115,19 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput&
       contact_map.shrinkToFit();
 
     info->contact_results = contacts;
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }
 
   info->color = "green";
   info->message = "Discrete contact check succeeded";
   info->return_value = 1;
-  info->elapsed_time = timer.elapsedSeconds();
   CONSOLE_BRIDGE_logDebug("%s", info->message.c_str());
   return info;
 }
 
 bool DiscreteContactCheckTask::operator==(const DiscreteContactCheckTask& rhs) const
 {
-  bool equal = true;
-  equal &= TaskComposerTask::operator==(rhs);
-  return equal;
+  return (TaskComposerTask::operator==(rhs));
 }
 bool DiscreteContactCheckTask::operator!=(const DiscreteContactCheckTask& rhs) const { return !operator==(rhs); }
 

tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp

@@ -29,7 +29,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
-#include <tesseract_common/timer.h>
 
 //#include <tesseract_process_managers/core/utils.h>
 #include <tesseract_task_composer/planning/nodes/fix_state_bounds_task.h>
@@ -73,14 +72,11 @@ FixStateBoundsTask::FixStateBoundsTask(std::string name,
 TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
                                                        OptionalTaskComposerExecutor /*executor*/) const
 {
-  auto info = std::make_unique<TaskComposerNodeInfo>(*this);
-
   // Get the problem
   auto& problem = dynamic_cast<PlanningTaskComposerProblem&>(*input.problem);
 
+  auto info = std::make_unique<TaskComposerNodeInfo>(*this);
   info->return_value = 0;
-  tesseract_common::Timer timer;
-  timer.start();
 
   // --------------------
   // Check that inputs are valid
@@ -89,7 +85,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
   if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))
   {
     info->message = "Input instruction to FixStateBounds must be a composite instruction";
-    info->elapsed_time = timer.elapsedSeconds();
     CONSOLE_BRIDGE_logError("%s", info->message.c_str());
     return info;
   }
@@ -130,7 +125,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
                 input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0]));
 
               info->message = "Failed to clamp to joint limits";
-              info->elapsed_time = timer.elapsedSeconds();
               return info;
             }
           }
@@ -157,7 +151,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
                 input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0]));
 
               info->message = "Failed to clamp to joint limits";
-              info->elapsed_time = timer.elapsedSeconds();
               return info;
             }
           }
@@ -176,7 +169,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
         info->color = "green";
         info->message = "FixStateBoundsTask found no MoveInstructions to process";
         info->return_value = 1;
-        info->elapsed_time = timer.elapsedSeconds();
         CONSOLE_BRIDGE_logWarn("%s", info->message.c_str());
         return info;
       }
@@ -205,7 +197,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
               input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0]));
 
             info->message = "Failed to clamp to joint limits";
-            info->elapsed_time = timer.elapsedSeconds();
             return info;
           }
         }
@@ -219,7 +210,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
       info->color = "green";
       info->message = "Successful, DISABLED";
       info->return_value = 1;
-      info->elapsed_time = timer.elapsedSeconds();
       return info;
     }
   }
@@ -229,7 +219,6 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input,
   info->color = "green";
   info->message = "Successful";
   info->return_value = 1;
-  info->elapsed_time = timer.elapsedSeconds();
   CONSOLE_BRIDGE_logDebug("FixStateBoundsTask succeeded");
   return info;
 }