Update to Tesseract 0.20 (#56)

* Updated motion planning node to be compatible with tesseract 0.20

* Updated the task composer config file

* Updated CI config

.github/workflows/ubuntu.yml

@@ -53,7 +53,7 @@ jobs:
           ROS_REPO: main
           BEFORE_INIT: ''
           PREFIX: ${{ github.repository }}_
-          DOCKER_IMAGE: ghcr.io/tesseract-robotics/tesseract_ros2:${{ matrix.distro }}-0.18
+          DOCKER_IMAGE: ghcr.io/tesseract-robotics/tesseract_ros2:${{ matrix.distro }}-0.20
           GIT_SUBMODULE_STRATEGY: normal
           BUILDER: colcon
           ADDITIONAL_DEBS: 'libxmlrpcpp-dev'

dependencies_tesseract.repos

@@ -5,23 +5,23 @@
 - git:
     local-name: tesseract
     uri: https://github.com/ros-industrial-consortium/tesseract.git
-    version: 0.18.1
+    version: 0.20.1
 - git:
     local-name: trajopt
     uri: https://github.com/ros-industrial-consortium/trajopt_ros.git
     version: 0.6.0
 - git:
     local-name: tesseract_planning
     uri: https://github.com/ros-industrial-consortium/tesseract_planning.git
-    version: 0.18.3
+    version: 0.20.2
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 5b5d38e202a87e5b34d12d14397ea6db3343f045
+    version: 0.20.0
 - git:
     local-name: tesseract_ros2
     uri: https://github.com/tesseract-robotics/tesseract_ros2.git
-    version: f7147aa20d8055953d49c6dcb7f144ef6de65d17
+    version: cfbc929e38382478f6e467d318858e817dae7749
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

snp_motion_planning/config/task_composer_plugins.yaml

@@ -3,10 +3,11 @@ DoneTask: &done
   class: DoneTaskFactory
   config:
     conditional: false
-AbortTask: &abort
-  class: AbortTaskFactory
+ErrorTask: &error
+  class: ErrorTaskFactory
   config:
     conditional: false
+    trigger_abort: true
 MinLengthTask: &min_length
   class: MinLengthTaskFactory
   config:
@@ -65,32 +66,32 @@ task_composer_plugins:
           outputs: [output_data]
           nodes:
             DoneTask: *done
-            AbortTask: *abort
+            ErrorTask: *error
             MinLengthTask: *min_length
             TrajOptMotionPlannerTask: *trajopt
             DiscreteContactCheckTask: *contact_check
             ConstantTCPSpeedTimeParameterizationTask: *constant_tcp_speed
             KinematicLimitsCheckTask: *limits_check
           edges:
             - source: MinLengthTask
-              destinations: [AbortTask, TrajOptMotionPlannerTask]
+              destinations: [ErrorTask, TrajOptMotionPlannerTask]
             - source: TrajOptMotionPlannerTask
-              destinations: [AbortTask, DiscreteContactCheckTask]
+              destinations: [ErrorTask, DiscreteContactCheckTask]
             - source: DiscreteContactCheckTask
-              destinations: [AbortTask, ConstantTCPSpeedTimeParameterizationTask]
-            - source: ConstantTCPSpeedTimeParameterizationTask # IterativeSplineParameterizationTask
-              destinations: [AbortTask, KinematicLimitsCheckTask]
+              destinations: [ErrorTask, ConstantTCPSpeedTimeParameterizationTask]
+            - source: ConstantTCPSpeedTimeParameterizationTask
+              destinations: [ErrorTask, KinematicLimitsCheckTask]
             - source: KinematicLimitsCheckTask
-              destinations: [AbortTask, DoneTask]
-          terminals: [AbortTask, DoneTask]
+              destinations: [ErrorTask, DoneTask]
+          terminals: [ErrorTask, DoneTask]
       SNPFreespacePipeline:
         class: GraphTaskFactory
         config:
           inputs: [input_data]
           outputs: [output_data]
           nodes:
             DoneTask: *done
-            AbortTask: *abort
+            ErrorTask: *error
             MinLengthTask: *min_length
             OMPLMotionPlannerTask:
               class: OMPLMotionPlannerTaskFactory
@@ -105,26 +106,26 @@ task_composer_plugins:
             KinematicLimitsCheckTask: *limits_check
           edges:
             - source: MinLengthTask
-              destinations: [AbortTask, OMPLMotionPlannerTask]
+              destinations: [ErrorTask, OMPLMotionPlannerTask]
             - source: OMPLMotionPlannerTask
-              destinations: [AbortTask, TrajOptMotionPlannerTask]
+              destinations: [ErrorTask, TrajOptMotionPlannerTask]
             - source: TrajOptMotionPlannerTask
-              destinations: [AbortTask, DiscreteContactCheckTask]
+              destinations: [ErrorTask, DiscreteContactCheckTask]
             - source: DiscreteContactCheckTask
-              destinations: [AbortTask, IterativeSplineParameterizationTask]
+              destinations: [ErrorTask, IterativeSplineParameterizationTask]
             - source: IterativeSplineParameterizationTask
-              destinations: [AbortTask, KinematicLimitsCheckTask]
+              destinations: [ErrorTask, KinematicLimitsCheckTask]
             - source: KinematicLimitsCheckTask
-              destinations: [AbortTask, DoneTask]
-          terminals: [AbortTask, DoneTask]
+              destinations: [ErrorTask, DoneTask]
+          terminals: [ErrorTask, DoneTask]
       SNPTransitionPipeline:
         class: GraphTaskFactory
         config:
           inputs: [input_data]
           outputs: [output_data]
           nodes:
             DoneTask: *done
-            AbortTask: *abort
+            ErrorTask: *error
             MinLengthTask: *min_length
             TrajOptMotionPlannerTask: *trajopt
             OMPLMotionPlannerTask:
@@ -139,26 +140,26 @@ task_composer_plugins:
             KinematicLimitsCheckTask: *limits_check
           edges:
             - source: MinLengthTask
-              destinations: [AbortTask, TrajOptMotionPlannerTask]
+              destinations: [ErrorTask, TrajOptMotionPlannerTask]
             - source: TrajOptMotionPlannerTask
               destinations: [OMPLMotionPlannerTask, DiscreteContactCheckTask]
             - source: OMPLMotionPlannerTask
-              destinations: [AbortTask, DiscreteContactCheckTask]
+              destinations: [ErrorTask, DiscreteContactCheckTask]
             - source: DiscreteContactCheckTask
-              destinations: [AbortTask, IterativeSplineParameterizationTask]
+              destinations: [ErrorTask, IterativeSplineParameterizationTask]
             - source: IterativeSplineParameterizationTask
-              destinations: [AbortTask, KinematicLimitsCheckTask]
+              destinations: [ErrorTask, KinematicLimitsCheckTask]
             - source: KinematicLimitsCheckTask
-              destinations: [AbortTask, DoneTask]
-          terminals: [AbortTask, DoneTask]
+              destinations: [ErrorTask, DoneTask]
+          terminals: [ErrorTask, DoneTask]
       SNPPipeline:
         class: GraphTaskFactory
         config:
           inputs: [input_data]
           outputs: [output_data]
           nodes:
             DoneTask: *done
-            AbortTask: *abort
+            ErrorTask: *error
             SimpleMotionPlannerTask:
               class: SimpleMotionPlannerTaskFactory
               config:
@@ -208,9 +209,9 @@ task_composer_plugins:
                     output_indexing: [output_data]
           edges:
             - source: SimpleMotionPlannerTask
-              destinations: [AbortTask, DescartesMotionPlannerTask]
+              destinations: [ErrorTask, DescartesMotionPlannerTask]
             - source: DescartesMotionPlannerTask
-              destinations: [AbortTask, RasterMotionTask]
+              destinations: [ErrorTask, RasterMotionTask]
             - source: RasterMotionTask
-              destinations: [AbortTask, DoneTask]
-          terminals: [AbortTask, DoneTask]
+              destinations: [ErrorTask, DoneTask]
+          terminals: [ErrorTask, DoneTask]

snp_motion_planning/src/planning_server.cpp

@@ -20,8 +20,6 @@
 #include <tesseract_rosutils/plotting.h>
 #include <tesseract_rosutils/utils.h>
 #include <tesseract_time_parameterization/isp/iterative_spline_parameterization.h>
-#include <tesseract_task_composer/core/task_composer_problem.h>
-#include <tesseract_task_composer/core/task_composer_input.h>
 #include <tesseract_task_composer/core/task_composer_plugin_factory.h>
 #include <tesseract_task_composer/planning/planning_task_composer_problem.h>
 #include <tesseract_task_composer/planning/profiles/contact_check_profile.h>
@@ -493,6 +491,8 @@ class PlanningServer
       auto task_name = get<std::string>(node_, TASK_NAME_PARAM);
       auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
       tesseract_planning::TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
+      if (!task)
+        throw std::runtime_error("Failed to create '" + task_name + "' task");
 
       // Save dot graph
       {
@@ -502,12 +502,11 @@ class PlanningServer
 
       const std::string input_key = task->getInputKeys().front();
       const std::string output_key = task->getOutputKeys().front();
-      tesseract_planning::TaskComposerDataStorage input_data;
-      input_data.setData(input_key, program);
-      tesseract_planning::TaskComposerProblem::UPtr problem =
-          std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(env_, input_data, profile_dict);
-      tesseract_planning::TaskComposerInput input(std::move(problem));
-      input.dotgraph = true;
+      auto task_data = std::make_shared<tesseract_planning::TaskComposerDataStorage>();
+      task_data->setData(input_key, program);
+      tesseract_planning::TaskComposerProblem::Ptr problem =
+          std::make_shared<tesseract_planning::PlanningTaskComposerProblem>(env_, profile_dict);
+      problem->dotgraph = true;
 
       // Update log level for debugging
       auto log_level = console_bridge::getLogLevel();
@@ -521,32 +520,34 @@ class PlanningServer
         {
           auto task_plugin_name = it->first.as<std::string>();
           std::ofstream f(tesseract_common::getTempPath() + task_plugin_name + ".dot");
-          factory.createTaskComposerNode(task_plugin_name)->dump(f);
+          tesseract_planning::TaskComposerNode::Ptr task = factory.createTaskComposerNode(task_plugin_name);
+          if (!task)
+            throw std::runtime_error("Failed to load task: '" + task_plugin_name + "'");
+          task->dump(f);
         }
       }
 
       // Run problem
-      tesseract_planning::TaskComposerFuture::UPtr exec_fut = executor->run(*task, input);
-      exec_fut->wait();
-
-      auto info_map = input.task_infos.getInfoMap();
+      tesseract_planning::TaskComposerFuture::UPtr result = executor->run(*task, problem, task_data);
+      result->wait();
 
       // Save the output dot graph
       {
         std::ofstream tc_out_results(tesseract_common::getTempPath() + task_name + "_results.dot");
-        static_cast<const tesseract_planning::TaskComposerGraph&>(*task).dump(tc_out_results, nullptr, info_map);
+        static_cast<const tesseract_planning::TaskComposerGraph&>(*task).dump(tc_out_results, nullptr,
+                                                                              result->context->task_infos.getInfoMap());
       }
 
       // Reset the log level
       console_bridge::setLogLevel(log_level);
 
       // Check for successful plan
-      if (!input.isSuccessful())
+      if (!result->context->isSuccessful() || result->context->isAborted())
         throw std::runtime_error("Failed to create motion plan");
 
       // Get results of successful plan
       tesseract_planning::CompositeInstruction program_results =
-          input.data_storage.getData(output_key).as<tesseract_planning::CompositeInstruction>();
+          result->context->data_storage->getData(output_key).as<tesseract_planning::CompositeInstruction>();
 
       // Convert to joint trajectory
       tesseract_common::JointTrajectory jt = toJointTrajectory(program_results);

snp_motion_planning/src/plugins/tasks/constant_tcp_speed_time_parameterization_task.cpp

@@ -7,7 +7,6 @@
 #include <console_bridge/console.h>
 #include <boost/serialization/string.hpp>
 
-#include <tesseract_common/timer.h>
 #include <tesseract_motion_planners/planner_utils.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
@@ -83,33 +82,23 @@ class ConstantTCPSpeedTimeParameterizationTask : public tesseract_planning::Task
 protected:
   friend struct tesseract_common::Serialization;
   friend class boost::serialization::access;
-  tesseract_planning::TaskComposerNodeInfo::UPtr runImpl(tesseract_planning::TaskComposerInput& input,
+  tesseract_planning::TaskComposerNodeInfo::UPtr runImpl(tesseract_planning::TaskComposerContext& context,
                                                          OptionalTaskComposerExecutor /*executor*/) const override final
   {
     auto info = std::make_unique<tesseract_planning::TaskComposerNodeInfo>(*this);
     info->return_value = 0;
 
-    if (input.isAborted())
-    {
-      info->message = "Aborted";
-      return info;
-    }
-
     // Get the problem
-    auto& problem = dynamic_cast<tesseract_planning::PlanningTaskComposerProblem&>(*input.problem);
-
-    tesseract_common::Timer timer;
-    timer.start();
+    auto& problem = dynamic_cast<tesseract_planning::PlanningTaskComposerProblem&>(*context.problem);
 
     // --------------------
     // Check that inputs are valid
     // --------------------
-    auto input_data_poly = input.data_storage.getData(input_keys_[0]);
+    auto input_data_poly = context.data_storage->getData(input_keys_[0]);
     if (input_data_poly.isNull() ||
         input_data_poly.getType() != std::type_index(typeid(tesseract_planning::CompositeInstruction)))
     {
       info->message = "Input results to constant TCP speed time parameterization must be a composite instruction";
-      info->elapsed_time = timer.elapsedSeconds();
       CONSOLE_BRIDGE_logError("%s", info->message.c_str());
       return info;
     }
@@ -122,15 +111,15 @@ class ConstantTCPSpeedTimeParameterizationTask : public tesseract_planning::Task
     profile = tesseract_planning::getProfileString(name_, profile, problem.composite_profile_remapping);
     auto cur_composite_profile = tesseract_planning::getProfile<ConstantTCPSpeedTimeParameterizationProfile>(
         name_, profile, *problem.profiles, std::make_shared<ConstantTCPSpeedTimeParameterizationProfile>());
-    cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides());
+    cur_composite_profile =
+        tesseract_planning::applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides());
 
     // Create data structures for checking for plan profile overrides
     auto flattened = ci.flatten(tesseract_planning::moveFilter);
     if (flattened.empty())
     {
       info->message = "Cartesian time parameterization found no MoveInstructions to process";
       info->return_value = 1;
-      info->elapsed_time = timer.elapsedSeconds();
       return info;
     }
 
@@ -148,15 +137,13 @@ class ConstantTCPSpeedTimeParameterizationTask : public tesseract_planning::Task
     {
       info->message =
           "Failed to perform constant TCP speed time parameterization for process input: " + ci.getDescription();
-      info->elapsed_time = timer.elapsedSeconds();
       return info;
     }
 
     info->color = "green";
     info->message = "Constant TCP speed time parameterization succeeded";
-    input.data_storage.setData(output_keys_[0], input_data_poly);
+    context.data_storage->setData(output_keys_[0], input_data_poly);
     info->return_value = 1;
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }
 

snp_motion_planning/src/plugins/tasks/kinematic_limits_check_task.cpp

@@ -80,33 +80,23 @@ class KinematicLimitsCheckTask : public tesseract_planning::TaskComposerTask
   friend struct tesseract_common::Serialization;
   friend class boost::serialization::access;
 
-  tesseract_planning::TaskComposerNodeInfo::UPtr runImpl(tesseract_planning::TaskComposerInput& input,
+  tesseract_planning::TaskComposerNodeInfo::UPtr runImpl(tesseract_planning::TaskComposerContext& context,
                                                          OptionalTaskComposerExecutor /*executor*/) const override final
   {
     auto info = std::make_unique<tesseract_planning::TaskComposerNodeInfo>(*this);
     info->return_value = 0;
 
-    if (input.isAborted())
-    {
-      info->message = "Aborted";
-      return info;
-    }
-
     // Get the problem
-    auto& problem = dynamic_cast<tesseract_planning::PlanningTaskComposerProblem&>(*input.problem);
-
-    tesseract_common::Timer timer;
-    timer.start();
+    auto& problem = dynamic_cast<tesseract_planning::PlanningTaskComposerProblem&>(*context.problem);
 
     // --------------------
     // Check that inputs are valid
     // --------------------
-    auto input_data_poly = input.data_storage.getData(input_keys_[0]);
+    auto input_data_poly = context.data_storage->getData(input_keys_[0]);
     if (input_data_poly.isNull() ||
         input_data_poly.getType() != std::type_index(typeid(tesseract_planning::CompositeInstruction)))
     {
       info->message = "Input results to kinimatic limits check must be a composite instruction";
-      info->elapsed_time = timer.elapsedSeconds();
       CONSOLE_BRIDGE_logError("%s", info->message.c_str());
       return info;
     }
@@ -119,15 +109,15 @@ class KinematicLimitsCheckTask : public tesseract_planning::TaskComposerTask
     profile = tesseract_planning::getProfileString(name_, profile, problem.composite_profile_remapping);
     auto cur_composite_profile = tesseract_planning::getProfile<KinematicLimitsCheckProfile>(
         name_, profile, *problem.profiles, std::make_shared<KinematicLimitsCheckProfile>());
-    cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides());
+    cur_composite_profile =
+        tesseract_planning::applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides());
 
     // Create data structures for checking for plan profile overrides
     auto flattened = ci.flatten(tesseract_planning::moveFilter);
     if (flattened.empty())
     {
       info->message = "Kinematic limits check found no MoveInstructions to process";
       info->return_value = 1;
-      info->elapsed_time = timer.elapsedSeconds();
       return info;
     }
 
@@ -195,7 +185,6 @@ class KinematicLimitsCheckTask : public tesseract_planning::TaskComposerTask
     info->color = "green";
     info->return_value = 1;
     info->message = "Kinematic limits check succeeded";
-    info->elapsed_time = timer.elapsedSeconds();
     return info;
   }