Add optional namespace field to task nodes

[marrts]

Currently there is a property in tesseract_task_composer that makes it difficult to add multiples of the same task in a pipeline. This PR addresses that issue by adding an optional "namespace" (we can maybe pick a better name, but this is what the addProfile method calls it) field to the task yaml object.

Going from:

Task:
  class: TaskFactory
  config:
    ...

To:

Task:
  class: TaskFactory
  namespace: TaskNS #(optional)
  config:
    ...

When adding a profile of a planner you need to specify a namespace, profile_name, and a shared_ptr to the profile definition. (see: here)

void addProfile(const std::string& ns, const std::string& profile_name, std::shared_ptr<const ProfileType> profile)

Why is this necessary?

Imagine a scenario that you want to do a freespace planner that tries to do the lowest effort planner first and then progressively goes to more robust planners as things fail. In this graph you might have multiple TrajOpt and/or Contact Check steps that you want to all be configured in the same way. When you get to a given task for a given waypoint a 2D lookup is performed to find the correct profile to be used. The task namespace and the name of the profile at the waypoint, then the profile object is pulled and applied to set up the actual planner used. Currently when creating a yaml file to set up a taskflow the namespace field is automatically generated from the task name as determined by the key in the yaml file. For example, suppose you define a task like this:

OMPLMotionPlannerTask:
  class: OMPLMotionPlannerTaskFactory
  config:
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: true

Here the namespace of the task will be OMPLMotionPlannerTask, but you are limited to only one task with this namespace. In my C++ code I'll make a definition of this profile so tesseract knows what to do whenever it comes across an OMPLMotionPlannerTask in a taskflow process. That will be defined by something like:

profile_dict->addProfile<tesseract_planning::OMPLPlanProfile>("OMPLMotionPlannerTask", PROFILE, profile);

In my above described scenario I need to do something that has multiple TrajOpt and multiple Contact Check tasks, so I need to set up my yaml file with something like:

TrajOptMotionPlannerTask1:
  class: TrajOptMotionPlannerTaskFactory
  config:
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: false
DiscreteContactCheckTask1:
  class: DiscreteContactCheckTaskFactory
  config:
    conditional: true
    inputs: [output_data]
TrajOptMotionPlannerTask2:
  class: TrajOptMotionPlannerTaskFactory
  config:
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: false
DiscreteContactCheckTask2:
  class: DiscreteContactCheckTaskFactory
  config:
    conditional: true
    inputs: [output_data]

This means in my C++ code I'll need to set up profiles for each of these:

// Add for first instance
profile_dict->addProfile<tesseract_planning::ContactCheckProfile>(
            "DiscreteContactCheckTask1", PROFILE,
            std::make_shared<tesseract_planning::ContactCheckProfile>(contact_check_lvs, contact_check_dist));
profile_dict->addProfile<tesseract_planning::TrajOptPlanProfile>("TrajOptMotionPlannerTask1", PROFILE,
                                                                         createTrajOptToolZFreePlanProfile());
profile_dict->addProfile<tesseract_planning::TrajOptCompositeProfile>("TrajOptMotionPlannerTask1", PROFILE,
                                                                      createTrajOptProfile());
                                                                      
// Add for first instance of task                                                                      
profile_dict->addProfile<tesseract_planning::ContactCheckProfile>(
            "DiscreteContactCheckTask2", PROFILE,
            std::make_shared<tesseract_planning::ContactCheckProfile>(contact_check_lvs, contact_check_dist));
profile_dict->addProfile<tesseract_planning::TrajOptPlanProfile>("TrajOptMotionPlannerTask2", PROFILE,
                                                                         createTrajOptToolZFreePlanProfile());
profile_dict->addProfile<tesseract_planning::TrajOptCompositeProfile>("TrajOptMotionPlannerTask2", PROFILE,
                                                                      createTrajOptProfile());
                                                                      

This can get really tedious and cumbersome if you want to be doing a lot of modifications to your pipeline, especially as you get to increasingly complex pipelines, forcing you to recompile code.

This proposed change simplifies this process on the C++ side by adding another, optional, field to the yaml constructor that specifies the namespace, turning my scenario into this (note there is no effect on how edges are made:

TrajOptMotionPlannerTask1:
  class: TrajOptMotionPlannerTaskFactory
  namespace: TrajOptMotionPlannerTask
  config:
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: false
DiscreteContactCheckTask1:
  class: DiscreteContactCheckTaskFactory
  namespace: DiscreteContactCheckTask
  config:
    conditional: true
    inputs: [output_data]
TrajOptMotionPlannerTask2:
  class: TrajOptMotionPlannerTaskFactory
  namespace: TrajOptMotionPlannerTask
  config:
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: false
DiscreteContactCheckTask2:
  class: DiscreteContactCheckTaskFactory
  namespace: DiscreteContactCheckTask
  config:
    conditional: true
    inputs: [output_data]

If a namespace field is detected, then the task namespace points to this string rather than the yaml key for the task. The following C++ code would then be sufficient:

// Add only once
profile_dict->addProfile<tesseract_planning::ContactCheckProfile>(
            "DiscreteContactCheckTask", PROFILE,
            std::make_shared<tesseract_planning::ContactCheckProfile>(contact_check_lvs, contact_check_dist));
profile_dict->addProfile<tesseract_planning::TrajOptPlanProfile>("TrajOptMotionPlannerTask", PROFILE,
                                                                         createTrajOptToolZFreePlanProfile());
profile_dict->addProfile<tesseract_planning::TrajOptCompositeProfile>("TrajOptMotionPlannerTask", PROFILE,
                                                                      createTrajOptProfile());

I can then add as many of these tasks to my pipeline without the need to recompile.

Why can't we just assign all tasks in a graph the same namespace?

It might seems like this unnecessarily over-complicates things. If we want to re-use a task of the same type then maybe we should assume all tasks of the same type in the same graph have the same namepsace, but this is not always true. You could have a scenario where you have multiple tasks of the same planner type that you want to try different parameters on. A common one I use that fits this description is water-falling Descartes from sparse sampling to dense sampling if I know I need to do a lot of motion plans where most will work with sparse sampling, but some require a denser sampling. Your resulting graph might look something like this:

In this scenario you want each Descartes solver to use a different profile.

Where's the catch?

There is one complication with my changes that I'm not a huge fan of. Here is an example of a graph with this change where it tries just TrajOpt first and then falls back to OMPL if that fails (Note I think there are potentially some issues with this pipeline, but it's just for demonstration sake)

The downside here is that you can see we have multiple diamond blocks with the same name, making debugging your graph potentially more difficult if you have a poorly formed YAML file. In the following image I've made an intentional typo where I pointed the second TrajOpt step to the first Contact Check instead of the second one, making the graph very confusing and hard to debug:

Here is that same graph without the namespace change (it seems marginally easier to debug):

I would like other people to weigh in on what they feel is the best approach.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (2e1e31c) 85.63% compared to head (12c7909) 85.62%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #433      +/-   ##
==========================================
- Coverage   85.63%   85.62%   -0.01%     
==========================================
  Files         221      221              
  Lines       14674    14679       +5     
==========================================
+ Hits        12566    12569       +3     
- Misses       2108     2110       +2     

see 2 files with indirect coverage changes

[rjoomen]

This seems like a useful change. Decoupling node names from profile namespaces also allows for more descriptive naming.

The catch with the duplicate names in the dot graph could fixed by showing the node name instead of the namespace, or maybe better show both (although that might crowd the figures). So then the graph nodes would show:

TrajOptMotionPlannerTask1
ns: TrajoptMotionPlannerTask
(uuid)
Inputs: [output_data]
Outputs: [output_data]

[Levi-Armstrong]

I think it is imported that the names be unique, but to accomplish the same thing you could update TaskComposerNode class to store the namespace and provide setters and getters. Then update the yaml parsing for the TaskComposerNode to extract the namespace if it exists otherwise use the name of the task. Update the dot to show the namespace if different. Then update the task to use the getter for the namespace instead of the name when retrieving profiles. @marrts What do you think?

[marrts]

I think it is imported that the names be unique, but to accomplish the same thing you could update TaskComposerNode class to store the namespace and provide setters and getters. Then update the yaml parsing for the TaskComposerNode to extract the namespace if it exists otherwise use the name of the task. Update the dot to show the namespace if different. Then update the task to use the getter for the namespace instead of the name when retrieving profiles. @marrts What do you think?

Okay, I'll work on this today. I agree that makes the most sense.

[marrts]

In starting to do this I am realizing this goes from a fully-backwards compatible change to a breaking change since I have to change the constructors of all the tasks to have a namespace field. @Levi-Armstrong, is this still what you want me to do?

[Levi-Armstrong]

In starting to do this I am realizing this goes from a fully-backwards compatible change to a breaking change since I have to change the constructors of all the tasks to have a namespace field. @Levi-Armstrong, is this still what you want me to do?

That should not be necessary if you make the namespace part of the config.

TrajOptMotionPlannerTask2:
  class: TrajOptMotionPlannerTaskFactory
  config:
    namespace: TrajOptMotionPlannerTask
    conditional: true
    inputs: [output_data]
    outputs: [output_data]
    format_result_as_input: false

[marrts]

Changed to work with namespace in the config. I'm not sure how we want to handle objects that inherit from tesseract_planning::MotionPlanner. I changed them all to use ns_ instead of name_ to match with the convention for getting profiles now used by all objects that inherit from TaskComposerTask, but anyone that has their own custom tasks will have to change to match this convention (although that is true for tasks that inherit from either MotionPlanner or TaskComposerTask)

Here is what it looks like in a dot graph now:

[Levi-Armstrong]

@marrts can you also updated the readme to document the namespace where it seems appropriate?

[marrts]

@marrts can you also updated the readme to document the namespace where it seems appropriate?

I added the namespace field everywhere it fit and marked it as optional. I also added an example of leveraging it to reuse a task configuration.

[rjoomen]

Would you be able to add this to the documentation?

[marrts]

Would you be able to add this to the documentation?

I added an example in the README of tesseract_task_composer. Is there somewhere else you had in mind?