Passing Instructions between taskflow nodes

[cpetersmeier]

Hello everyone!

Setup

I am planning a trajectory where I want to compute the second segment B before the first A.
My Taskflow generator has three Tasks B -> B' -> A.

The input composite is of the format:

Composite
{
   Composite - A
     Start:      start
   Composite - B
     Start:      start (this gets modified during planning -> need to extract first MoveInstruction of results)
     Plan:       goal
}

Question

How do I pass the first move instruction of segment B as the goal of segment A? Currently I am getting a segmentation fault error.

Code

Since the input instructions for the TaskInput are const, I figured I create a subcontainer of the TaskInput datastructure for task A and write a taskflow node B' that uses setEndInstruction() on the subContainers' reference with the first MoveInstruction of task B.

Then the motion_planner_task_generator should add the end instruction to the planner requests instructions.

Here is how I construct the subcontainer. Note that I am not directly going from B -> B' but I added a debug printer task.

TaskInput A_input = input[0];

tf::Task extract_point_task = container.taskflow->emplace(
  [&](){positionalHandover(input, A_input); }
).name("Positional Handover Task");
extract_point_task.succeed(printer_task);

Here is my code of B':

void CustomTaskflow::positionalHandover(TaskInput& input, TaskInput& A_input){
  CompositeInstruction& results = input.getResults()->as<CompositeInstruction>();  // Segmentation fault occurs here in isCompositeInstruction() -> getType()

  CompositeInstruction& B_results = results.at(1).as<CompositeInstruction>();

  // get the first move instruction from this path
  MoveInstruction& first_move_instruction = *getFirstMoveInstruction(B_results);
  A_input.setEndInstruction(first_move_instruction);
  
  return;
}

I tried using the latest taskflow v3.4, which supports passing additional data into the tasks but hat no luck with that either.

Debug:   worker 0 ready to run Positional Handover Task
[app_node-1] process has died [pid 113693, exit code -11 ... (log files, etc ..)]

[cpetersmeier]

Actually, the segmentation fault can be mitigated by capturing the input variable by value

[input, &A_input](){positionalHandover(input, A_input); }

and just using a reference for the A_input subcontainer.

void CustomTaskflow::positionalHandover(TaskInput input, TaskInput& A_input){

[Levi-Armstrong]

You will need to write your own graph taskflow. An example of this can be seen by looking at the raster graph taskflow.

[cpetersmeier]

Thanks for the quick reply! The code above is contained in my own graph taskflow. I looked at this exampe:

tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_global_taskflow.cpp

Lines 149 to 168 in 86ac769

	void RasterOnlyGlobalTaskflow::globalPostProcess(TaskInput input)
	{
	if (input.isAborted())
	return;
	auto& results = input.getResults()->as<CompositeInstruction>();
	auto& composite = results.at(0).as<CompositeInstruction>();
	composite.setStartInstruction(results.getStartInstruction());
	composite.setManipulatorInfo(results.getManipulatorInfo());
	for (std::size_t i = 1; i < results.size(); ++i)
	{
	auto& composite0 = results.at(i - 1).as<CompositeInstruction>();
	MoveInstruction lmi = *getLastMoveInstruction(composite0);
	lmi.setMoveType(MoveInstructionType::START);
	auto& composite1 = results.at(i).as<CompositeInstruction>();
	composite1.setStartInstruction(lmi);
	composite1.setManipulatorInfo(results.getManipulatorInfo());
	}
	}

However when I do a setEndInstruction() in my positionalHandover task and try to read it out when I pass this modified TaskInput to the next motion planner graph node, I get a NullInstruction.

I think the example modifies the seed of the next planner? Would this accomplish the same effect as adding an end instruction which gets then added to the input instructions here?

tesseract_planning/tesseract_process_managers/src/task_generators/motion_planner_task_generator.cpp

Line 137 in 86ac769

lpi->setWaypoint(end_instruction.as<MoveInstruction>().getWaypoint());

[Levi-Armstrong]

Take a look at this section of code. You want to provide it with the index instead of the actual waypoint. There is a comment explaining why this has to be done, but in short is because the result data structure has not been generated so you are unable to reference it directly.

tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_taskflow.cpp

Lines 118 to 143 in 86ac769

	for (std::size_t input_idx = 2; input_idx < input.size() - 2; input_idx += 2)
	{
	// This use to extract the start and end, but things were changed so the seed is generated as part of the
	// taskflow. So the seed is only a skeleton and does not contain move instructions. So instead we provide the
	// composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more
	// robust because planners could modify composite size, which is rare but does happen when using OMPL where it is
	// not possible to simplify the trajectory to the desired number of states.
	TaskInput transition_input = input[input_idx];
	transition_input.setStartInstruction(std::vector<std::size_t>({ input_idx - 1 }));
	transition_input.setEndInstruction(std::vector<std::size_t>({ input_idx + 1 }));
	TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow(
	transition_input,
	[=]() { successTask(input, name_, transition_input.getInstruction()->getDescription(), done_cb); },
	[=]() { failureTask(input, name_, transition_input.getInstruction()->getDescription(), error_cb); });
	auto transition_step = container.taskflow->composed_of(*(sub_container.taskflow))
	.name("Transition #" + std::to_string(transition_idx + 1) + ": " +
	transition_input.getInstruction()->getDescription());
	container.containers.push_back(std::move(sub_container));
	// Each transition is independent and thus depends only on the adjacent rasters
	transition_step.succeed(tasks[transition_idx]);
	transition_step.succeed(tasks[transition_idx + 1]);
	transition_idx++;
	}

[Levi-Armstrong]

This is the index into the vector of the high level composite instruction. In your case I believe the this should be index 1 for B.

[cpetersmeier]

Awesome, thanks a lot for your help and quick response!

If I may ask: Is there a way to let the rviz trajectory visualization plugin display the full path? Currently it only loops through the B section.

I also flattened out all MoveInstructions of the result composite by default for the toolpath extraction, otherwise I would not get a path displayed in rviz.

[Levi-Armstrong]

I recommend trying the TesseractWorkbench plugin which may have what you need. This provides more details.