Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Move TaskComposerProblem input to base class and change type to tesseract_common::AnyPoly #407

Merged
merged 1 commit into from
Nov 6, 2023
Merged

Move TaskComposerProblem input to base class and change type to tesseract_common::AnyPoly #407

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion tesseract_examples/src/basic_cartesian_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@ bool BasicCartesianExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

if (plotter_ != nullptr && plotter_->isConnected())
plotter_->waitForInput("Hit Enter to solve for trajectory.");
Expand Down
4 changes: 2 additions & 2 deletions tesseract_examples/src/car_seat_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,7 @@ bool CarSeatExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
Expand Down Expand Up @@ -386,7 +386,7 @@ bool CarSeatExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/freespace_hybrid_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ bool FreespaceHybridExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/freespace_ompl_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ bool FreespaceOMPLExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/glass_upright_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,7 @@ bool GlassUprightExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

if (plotter_ != nullptr && plotter_->isConnected())
plotter_->waitForInput("Hit Enter to solve for trajectory.");
Expand Down
4 changes: 2 additions & 2 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ bool PickAndPlaceExample::run()

// Create Task Composer Problem
auto pick_problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
pick_problem->input_instruction = pick_program;
pick_problem->input = pick_program;

// Solve task
TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem));
Expand Down Expand Up @@ -344,7 +344,7 @@ bool PickAndPlaceExample::run()

// Create Task Composer Problem
auto place_problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
place_problem->input_instruction = place_program;
place_problem->input = place_program;

// Solve task
TaskComposerFuture::UPtr place_future = executor->run(*place_task, std::move(place_problem));
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -233,7 +233,7 @@ bool PuzzlePieceAuxillaryAxesExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/puzzle_piece_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ bool PuzzlePieceExample::run()

// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
problem->input = program;

// Solve task
tesseract_common::Timer stopwatch;
Expand Down
4 changes: 4 additions & 0 deletions tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_common/serialization.h>
#include <tesseract_common/any_poly.h>
#include <tesseract_task_composer/core/task_composer_data_storage.h>

namespace tesseract_planning
Expand All @@ -58,6 +59,9 @@ struct TaskComposerProblem
/** @brief Indicate if dotgraph should be provided */
bool dotgraph{ false };

/** @brief The problem input */
tesseract_common::AnyPoly input;

/**
* @brief Clone the planning problem
* @return A clone of the planning problem
Expand Down
2 changes: 2 additions & 0 deletions tesseract_task_composer/core/src/task_composer_problem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const
bool equal = true;
equal &= name == rhs.name;
equal &= dotgraph == rhs.dotgraph;
equal &= input == input;
return equal;
}

Expand All @@ -56,6 +57,7 @@ void TaskComposerProblem::serialize(Archive& ar, const unsigned int /*version*/)
{
ar& boost::serialization::make_nvp("name", name);
ar& boost::serialization::make_nvp("dotgraph", dotgraph);
ar& boost::serialization::make_nvp("input", dotgraph);
}

} // namespace tesseract_planning
Expand Down
3 changes: 0 additions & 3 deletions ...mposer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,6 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem
/** @brief The Profiles to use */
ProfileDictionary::ConstPtr profiles;

/** @brief The problem input instruction */
InstructionPoly input_instruction;

/**
* @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a
* given motion planner.
Expand Down
6 changes: 3 additions & 3 deletions tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp
View file
Open in desktop
Copy link
Contributor

@marip8 marip8 Nov 6, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since all tasks will now be required to put their input data in the problem directly, would it make more since for the executor to put the input data in the correct place in the internal data storage object, rather than having this separate task* do it? Otherwise, it seems like all pipelines from here on out will need to have this as the first task in the pipeline

* I understand this this task also checks that the input is a composite instruction, but it's other major function is to put the input data into the correct spot in the data storage object for downstream tasks

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lets add it as an issue to investigate if this is the right thing to do.

Original file line number Diff line number Diff line change
Expand Up @@ -66,18 +66,18 @@ TaskComposerNodeInfo::UPtr ProcessPlanningInputTask::runImpl(TaskComposerContext

auto info = std::make_unique<TaskComposerNodeInfo>(*this);

if (!problem.input_instruction.isCompositeInstruction())
if (problem.input.isNull() || problem.input.getType() != std::type_index(typeid(CompositeInstruction)))
{
info->color = "red";
info->message = "Input instructon is not a Composite Instruction, aborting...";
info->message = "Input is not a Composite Instruction, aborting...";
info->return_value = 0;

// Abort
context.abort(uuid_);
return info;
}

context.data_storage->setData(output_keys_[0], problem.input_instruction.as<CompositeInstruction>());
context.data_storage->setData(output_keys_[0], problem.input.as<CompositeInstruction>());

info->color = "green";
info->message = "Successful";
Expand Down
2 changes: 0 additions & 2 deletions tesseract_task_composer/planning/src/planning_task_composer_problem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,6 @@ bool PlanningTaskComposerProblem::operator==(const PlanningTaskComposerProblem&
equal &= tesseract_common::pointersEqual(env, rhs.env);
equal &= manip_info == rhs.manip_info;
// equal &= tesseract_common::pointersEqual(profiles, rhs.profiles);
equal &= input_instruction == rhs.input_instruction;
equal &= move_profile_remapping == rhs.move_profile_remapping;
equal &= composite_profile_remapping == rhs.composite_profile_remapping;
return equal;
Expand All @@ -118,7 +117,6 @@ void PlanningTaskComposerProblem::serialize(Archive& ar, const unsigned int /*ve
ar& boost::serialization::make_nvp("manip_info", manip_info);
/** @todo Fix after profiles are serializable */
// ar& boost::serialization::make_nvp("profiles", profiles);
ar& boost::serialization::make_nvp("input_instruction", input_instruction);
ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping);
ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping);
}
Expand Down
28 changes: 14 additions & 14 deletions tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,9 +91,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) //
EXPECT_TRUE(problem.move_profile_remapping.empty());
EXPECT_TRUE(problem.composite_profile_remapping.empty());
EXPECT_TRUE(problem.profiles != nullptr);
EXPECT_TRUE(problem.input_instruction.isNull());
problem.input_instruction = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input_instruction.isNull());
EXPECT_TRUE(problem.input.isNull());
problem.input = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input.isNull());

// Test clone
auto clone = problem.clone();
Expand All @@ -110,9 +110,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) //
EXPECT_TRUE(problem.move_profile_remapping.empty());
EXPECT_TRUE(problem.composite_profile_remapping.empty());
EXPECT_TRUE(problem.profiles != nullptr);
EXPECT_TRUE(problem.input_instruction.isNull());
problem.input_instruction = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input_instruction.isNull());
EXPECT_TRUE(problem.input.isNull());
problem.input = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input.isNull());

// Test clone
auto clone = problem.clone();
Expand All @@ -129,9 +129,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) //
EXPECT_TRUE(problem.move_profile_remapping.empty());
EXPECT_TRUE(problem.composite_profile_remapping.empty());
EXPECT_TRUE(problem.profiles != nullptr);
EXPECT_TRUE(problem.input_instruction.isNull());
problem.input_instruction = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input_instruction.isNull());
EXPECT_TRUE(problem.input.isNull());
problem.input = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input.isNull());

// Test clone
auto clone = problem.clone();
Expand All @@ -148,9 +148,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) //
EXPECT_TRUE(problem.move_profile_remapping.empty());
EXPECT_TRUE(problem.composite_profile_remapping.empty());
EXPECT_TRUE(problem.profiles != nullptr);
EXPECT_TRUE(problem.input_instruction.isNull());
problem.input_instruction = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input_instruction.isNull());
EXPECT_TRUE(problem.input.isNull());
problem.input = test_suite::freespaceExampleProgramABB();
EXPECT_FALSE(problem.input.isNull());

// Test clone
auto clone = problem.clone();
Expand Down Expand Up @@ -2554,7 +2554,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) //
// Create problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->dotgraph = true;
problem->input_instruction = test_suite::rasterExampleProgram();
problem->input = test_suite::rasterExampleProgram();

// Solve raster plan
auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
Expand Down Expand Up @@ -3025,7 +3025,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests)
// Create problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->dotgraph = true;
problem->input_instruction = test_suite::rasterOnlyExampleProgram();
problem->input = test_suite::rasterOnlyExampleProgram();

// Solve raster plan
auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
Expand Down
Loading