Task Composer Loading Issue

[marip8]

Hi!

I'm using tesseract as the motion planner as part of the Scan and Plan workshop code. I am using ROS2 Humble on Ubuntu 22.04, V0.18.1 on tesseract and 0.18.3 on tesseract_planning

This is what my task_composer_plugins.yaml looks like

task_composer_plugins:
  search_paths:
    - /usr/local/lib
    - /home/srsidd/ws/install/snp_motion_planning/lib/
    - /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/
  search_libraries:
    - tesseract_task_composer_factories
    - snp_motion_planning_tasks
  executors:
    default: TaskflowExecutor
    plugins:
      TaskflowExecutor:
        class: TaskflowTaskComposerExecutorFactory
        config:
          threads: 8
  tasks:
    plugins:
      SNPCartesianPipeline:
        class: GraphTaskFactory
        config:
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: false
            #DescartesMotionPlannerTask:
            #  class: DescartesFMotionPlannerTaskFactory
            #  config:
            #    conditional: true
            #    inputs: [output_data]
            #    outputs: [output_data]
            #    format_result_as_input: false
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            # # ConstantTCPSpeedTimeParameterizationTask:
            # #   class: ConstantTCPSpeedTimeParameterizationTaskFactory
            # #   config:
            # #     conditional: true
            # #     inputs: [output_data]
            # #     outputs: [output_data]
            # # KinematicLimitsCheckTask:
            # #   class: KinematicLimitsCheckTaskFactory
            # #   config:
            # #     conditional: true
            # #     inputs: [output_data]
            # #     outputs: [output_data]
          edges:
            - source: MinLengthTask
            #  destinations: [AbortTask, DescartesMotionPlannerTask]
              destinations: [AbortTask, TrajOptMotionPlannerTask]
            #- source: DescartesMotionPlannerTask
            #  destinations: [AbortTask, TrajOptMotionPlannerTask]
            #  destinations: [AbortTask, DiscreteContactCheckTask]
            - source: TrajOptMotionPlannerTask
              destinations: [AbortTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [AbortTask, DoneTask]
            # - source: ConstantTCPSpeedTimeParameterizationTask # IterativeSplineParameterizationTask
            #   destinations: [AbortTask, KinematicLimitsCheckTask]
            # - source: KinematicLimitsCheckTask
            #   destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      SNPFreespacePipeline:
        class: GraphTaskFactory
        config:
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            OMPLMotionPlannerTask:
              class: OMPLMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
            # KinematicLimitsCheckTask:
            #   class: KinematicLimitsCheckTaskFactory
            #   config:
            #     conditional: true
            #     inputs: [output_data]
            #     outputs: [output_data]
          edges:
            - source: MinLengthTask
            #  destinations: [AbortTask, TrajOptMotionPlannerTask]
              destinations: [AbortTask, OMPLMotionPlannerTask]
            - source: OMPLMotionPlannerTask
              destinations: [AbortTask, TrajOptMotionPlannerTask]
            - source: TrajOptMotionPlannerTask
              destinations: [AbortTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [AbortTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [AbortTask, DoneTask]
            # - source: KinematicLimitsCheckTask
            #   destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      SNPTransitionPipeline:
        class: GraphTaskFactory
        config:
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            OMPLMotionPlannerTask:
              class: OMPLMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
            # KinematicLimitsCheckTask:
            #   class: KinematicLimitsCheckTaskFactory
            #   config:
            #     conditional: true
            #     inputs: [output_data]
            #     outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [AbortTask, TrajOptMotionPlannerTask]
            - source: TrajOptMotionPlannerTask
              destinations: [OMPLMotionPlannerTask, DiscreteContactCheckTask]
            - source: OMPLMotionPlannerTask
              destinations: [AbortTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [AbortTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [AbortTask, DoneTask]
            # - source: KinematicLimitsCheckTask
            #   destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      SNPPipeline:
        class: GraphTaskFactory
        config:
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                freespace:
                  task: SNPFreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    output_remapping:
                      output_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                raster:
                  task: SNPCartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    output_remapping:
                      output_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: SNPTransitionPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    output_remapping:
                      output_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [AbortTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [AbortTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]

However when I run and try to plan for a generated set of tool paths, I get the following error -

[snp_motion_planning_node-6] Debug:   Failed to find or load library: /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so with error: Bad file descriptor
[snp_motion_planning_node-6] Debug:   Failed to find or load library: /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so with error: Bad file descriptor
[snp_motion_planning_node-6] Debug:   Failed to find or load library: /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so with error: Bad file descriptor
[snp_motion_planning_node-6] Debug:   Failed to find or load library: /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so with error: Bad file descriptor
[snp_motion_planning_node-6] Debug:   Failed to find or load library: /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so with error: Bad file descriptor

Any idea why it's not able to find the library and what I'm missing?

Originally posted by @srsidd here

[marip8]

Can you confirm that the library /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/libsnp_motion_planning_tasks.so actually exists on your machine at this path? If it does, it's probably a linking error (where this library is trying to dynamically link against another library that doesn't exist or can't be found) or an issue where this particular library contains an undefined symbol.

@marrts have you had this issue on your update branch?

[marrts]

You would not expect that library to exist at that path. libsnp_motion_planning_tasks.so will get generated into the install space of the snp_motion_planning package, not tesseract_task_composer. This debug message is printed out whenever a library search path does not find the library. No message is printed out for when it DOES find the library. I tracked it down one time and if I remember correctly I think this message goes all the way down to boost_plugin_loader. You can see all the specified search paths at the top of the yaml file.

search_paths:
- /usr/local/lib
- /home/srsidd/ws/install/snp_motion_planning/lib/
- /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/

This message is only cause for concern if a motion plan actually fails due to a library linking issue.

[srsidd]

The library is part of snp_motion_planning. I see it under /home/srsidd/ws/install/snp_motion_planning/lib/libsnp_motion_planning_plugins.so. I thought by specifying home/srsidd/ws/install/snp_motion_planning/lib/ in the search paths it would link correctly?

For reference the task_composer.yaml file from the port also specifies it same way (https://github.com/marrts/scan_n_plan_workshop/blob/update/tesseract_0.16/snp_motion_planning/config/task_composer_plugins.yaml#L6)

[srsidd]

Ah, I see. In that case any idea why the motion planning node crashes right after? I don't see a plan being generated but the log says the planner succeeded. I assumed it was because the libraries weren't being found -

[snp_motion_planning_node-6] Debug:   Environment, getJointGroup(manipulator) cache miss!
[snp_motion_planning_node-6] Debug:   Environment, getGroupJointNames(manipulator) cache miss!
[snp_motion_planning_node-6] Debug:   Environment, getJointGroup(manipulator) cache hit!
[snp_motion_planning_node-6] Debug:   Environment, getKinematicGroup(manipulator, ) cache miss!
[snp_motion_planning_node-6] Debug:   Environment, getGroupJointNames(manipulator) cache hit!
[snp_motion_planning_node-6] Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
[snp_motion_planning_node-6] Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
[snp_motion_planning_node-6] Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
[snp_motion_planning_node-6] Debug:   Motion Planner process succeeded
[snp_motion_planning_node-6] Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
[snp_motion_planning_node-6] Debug:   Descartes took 0.0068 seconds to build vertices.
[snp_motion_planning_node-6] Debug:   Descartes took 0.0000 seconds to build edges.
[snp_motion_planning_node-6] Info:    No failed vertices
[snp_motion_planning_node-6] Info:    No failed edges
[snp_motion_planning_node-6] Debug:   Descartes took 0.0000 seconds to search graph for solution with cost 37.6370.
[snp_motion_planning_node-6] Debug:   Motion Planner process succeeded
[ERROR] [snp_motion_planning_node-6]: process has died [pid 82352, exit code -11, cmd '/home/srsidd/ws/install/snp_motion_planning/lib/snp_motion_planning/snp_motion_planning_node --ros-args --params-file /tmp/launch_params_kdog47rm --params-file /tmp/launch_params_3gaqc87l --params-file /tmp/launch_params_h5rnc6m8 --params-file /tmp/launch_params_0k0w3v6m --params-file /tmp/launch_params_ggveq_qk'].

[marip8]

You can see all the specified search paths at the top of the yaml file.

search_paths:

• /usr/local/lib
• /home/srsidd/ws/install/snp_motion_planning/lib/
• /home/srsidd/tesseract_ws/install/tesseract_task_composer/lib/

FWIW, I don't think any of these are actually necessary. The plugin loader searches in "system-level" directories by default, which includes all of these directories when you source your workspace install/setup.bash

[marrts]

Ah, I see. In that case any idea why the motion planning node crashes right after? I don't see a plan being generated but the log says the planner succeeded. I assumed it was because the libraries weren't being found -

It's not immediately obvious why this is failing. I'd recommend doing the following:

• Launch with verbose set to true ros2 launch snp_automate_2022 start.launch.xml verbose:=true
• Once you have run the motion plan request before it crashes it will generate .dot files in your /tmp directory which can be viewed with an application like xdot (sudo apt install xdot).
  • /tmp/ScanNPlanPipeline.dot
  • /tmp/SNPTransitionPipeline.dot
  • /tmp/SNPFreespacePipeline.dot
  • /tmp/SNPCartesianPipeline.dot
• They should resemble something like this:
  
  It should have the following characteristics:
• There should only be a single node that does not have any arrows pointing to it, if there is more than one of these then you messed up somewhere in specifying your edges.
• Each node should only have two outputs, if there are more than two then you messed up somewhere in specifying your edges.
• No node should be sitting by itself
• An arrow should never loop backwards, leading to a potentially infinite process

Note, depending on your version of Tesseract it might look different than mine, but the overall rules still apply

[marrts]

It's not immediately obvious why this is failing. I'd recommend doing the following:

I'll also add that it appears to be failing when going into the raster planner, which the first step is to run SNPCartesianPipeline on each of the rasters. The Debug: Motion Planner process succeeded message you are seeing I believe is actually from Descartes succeeding, not the full process finishing. Descartes succeeding indicates your overall input is probably good

[srsidd]

I tried looking at the dot files. I see all 4 files, the first three SNPTransitionPipeline.dot and SNPFreespacePipeline.dot, SNPCartesianPipeline.dot look ok and adhere with all the characteristics you mentioned.

However I couldn't visualize ScanNPlanPipeline.dot. It threw an error -

~@ubuntu xdot /tmp/ScanNPlanPipeline.dot 
Error: <stdin>: syntax error in line 17

Looking at the file, it isn't complete and just stops midway -

digraph TaskComposer {
subgraph cluster_40d64fa794b24144af682cbae667be08 {
 color=black;
 label = "SNPPipeline\n(40d64fa7-94b2-4144-af68-2cbae667be08)";
node_8f649aa53e394a6eaa516146a2d779b9 [label="DoneTask\n(8f649aa5-3e39-4a6e-aa51-6146a2d779b9)", color=black, fillcolor=white, style=filled];

node_94faad7acbd64f60be6cb346c2191016 [shape=diamond, label="SimpleMotionPlannerTask\n(94faad7a-cbd6-4f60-be6c-b346c2191016)", color=black, fillcolor=white, style=filled];
node_94faad7acbd64f60be6cb346c2191016 -> node_c0e62796d5994722bf4dc51ed514f405 [style=dashed, label="[0]"];
node_94faad7acbd64f60be6cb346c2191016 -> node_ce78782ea3ea4edba0cd438ab0c7dd68 [style=dashed, label="[1]"];

node_b6bbfaa4808249e0b5a287bff0d87d02 [shape=diamond, label="RasterMotionTask\n(b6bbfaa4-8082-49e0-b5a2-87bff0d87d02)", color=black, fillcolor=white, style=filled];
node_b6bbfaa4808249e0b5a287bff0d87d02 -> node_c0e62796d5994722bf4dc51ed514f405 [style=dashed, label="[0]"];
node_b6bbfaa4808249e0b5a287bff0d87d02 -> node_8f649aa53e394a6eaa516146a2d779b9 [style=dashed, label="[1]"];

node_c0e62796d5994722bf4dc51ed514f405 [label="AbortTask\n(c0e62796-d599-4722-bf4d-c51ed514f405)", color=black, fillcolor=white, style=filled];

I added two brackets at the end and I see a problem and now see this -

This is interesting since the task_composes_plugins.yaml is exactly from the fork which the PR is from (https://github.com/marrts/scan_n_plan_workshop/blob/update/tesseract_0.16/snp_motion_planning/config/task_composer_plugins.yaml)