Skip to content

Commit

Permalink
Rename TaskComposerInput to TaskComposerContext and simplify interfac…
Browse files Browse the repository at this point in the history 
…es (#379)
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong authored Sep 19, 2023
1 parent 7bfc825 commit f216b4d
Show file tree
Hide file tree
Showing 94 changed files with 930 additions and 1,591 deletions.
15 changes: 7 additions & 8 deletions tesseract_examples/src/basic_cartesian_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>

#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>

#include <tesseract_geometry/impl/octree.h>
Expand Down Expand Up @@ -244,20 +244,19 @@ bool BasicCartesianExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

if (plotter_ != nullptr && plotter_->isConnected())
plotter_->waitForInput("Hit Enter to solve for trajectory.");

// Solve task
tesseract_common::Timer stopwatch;
stopwatch.start();
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

stopwatch.stop();
Expand All @@ -267,7 +266,7 @@ bool BasicCartesianExample::run()
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand All @@ -276,7 +275,7 @@ bool BasicCartesianExample::run()
}

CONSOLE_BRIDGE_logInform("Final trajectory is collision free");
return input.isSuccessful();
return future->context->isSuccessful();
}

} // namespace tesseract_examples
28 changes: 13 additions & 15 deletions tesseract_examples/src/car_seat_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/profile_dictionary.h>
#include <tesseract_command_language/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>

#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
Expand Down Expand Up @@ -302,24 +302,23 @@ bool CarSeatExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

// Solve task
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

if (!input.isSuccessful())
if (!future->context->isSuccessful())
return false;

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand Down Expand Up @@ -391,24 +390,23 @@ bool CarSeatExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

// Solve task
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

if (!input.isSuccessful())
if (!future->context->isSuccessful())
return false;

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand Down
15 changes: 7 additions & 8 deletions tesseract_examples/src/freespace_hybrid_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
#include <tesseract_motion_planners/core/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
#include <tesseract_visualization/markers/toolpath_marker.h>

Expand Down Expand Up @@ -180,22 +180,21 @@ bool FreespaceHybridExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

// Solve task
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand All @@ -204,6 +203,6 @@ bool FreespaceHybridExample::run()
}

CONSOLE_BRIDGE_logInform("Final trajectory is collision free");
return input.isSuccessful();
return future->context->isSuccessful();
}
} // namespace tesseract_examples
15 changes: 7 additions & 8 deletions tesseract_examples/src/freespace_ompl_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
#include <tesseract_motion_planners/core/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
#include <tesseract_visualization/markers/toolpath_marker.h>

Expand Down Expand Up @@ -180,22 +180,21 @@ bool FreespaceOMPLExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

// Solve task
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand All @@ -204,6 +203,6 @@ bool FreespaceOMPLExample::run()
}

CONSOLE_BRIDGE_logInform("Final trajectory is collision free");
return input.isSuccessful();
return future->context->isSuccessful();
}
} // namespace tesseract_examples
15 changes: 7 additions & 8 deletions tesseract_examples/src/glass_upright_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/move_instruction.h>
#include <tesseract_command_language/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
#include <tesseract_visualization/markers/toolpath_marker.h>

Expand Down Expand Up @@ -237,20 +237,19 @@ bool GlassUprightExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

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

if (plotter_ != nullptr && plotter_->isConnected())
plotter_->waitForInput("Hit Enter to solve for trajectory.");

// Solve process plan
tesseract_common::Timer stopwatch;
stopwatch.start();
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

stopwatch.stop();
Expand All @@ -260,7 +259,7 @@ bool GlassUprightExample::run()
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand All @@ -270,6 +269,6 @@ bool GlassUprightExample::run()
}

CONSOLE_BRIDGE_logInform("Final trajectory is collision free");
return input.isSuccessful();
return future->context->isSuccessful();
}
} // namespace tesseract_examples
32 changes: 16 additions & 16 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/planning/profiles/contact_check_profile.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
#include <tesseract_visualization/markers/toolpath_marker.h>

Expand Down Expand Up @@ -227,25 +227,24 @@ bool PickAndPlaceExample::run()
const std::string pick_output_key = pick_task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage pick_input_data;
pick_input_data.setData(pick_input_key, pick_program);
auto pick_input_data = std::make_unique<TaskComposerDataStorage>();
pick_input_data->setData(pick_input_key, pick_program);

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

// Solve task
TaskComposerInput pick_input(std::move(pick_problem));
TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, pick_input);
TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem), std::move(pick_input_data));
pick_future->wait();

if (!pick_input.isSuccessful())
if (!pick_future->context->isSuccessful())
return false;

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = pick_input.data_storage.getData(pick_output_key).as<CompositeInstruction>();
auto ci = pick_future->context->data_storage->getData(pick_output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand Down Expand Up @@ -278,7 +277,8 @@ bool PickAndPlaceExample::run()
env_->applyCommands(cmds);

// Get the last move instruction
CompositeInstruction pick_composite = pick_input.data_storage.getData(pick_output_key).as<CompositeInstruction>();
CompositeInstruction pick_composite =
pick_future->context->data_storage->getData(pick_output_key).as<CompositeInstruction>();
const MoveInstructionPoly* pick_final_state = pick_composite.getLastMoveInstruction();

// Retreat to the approach pose
Expand Down Expand Up @@ -348,25 +348,25 @@ bool PickAndPlaceExample::run()
const std::string place_output_key = pick_task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage place_input_data;
place_input_data.setData(place_input_key, place_program);
auto place_input_data = std::make_unique<TaskComposerDataStorage>();
place_input_data->setData(place_input_key, place_program);

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

// Solve task
TaskComposerInput place_input(std::move(place_problem));
TaskComposerFuture::UPtr place_future = executor->run(*place_task, place_input);
TaskComposerFuture::UPtr place_future =
executor->run(*place_task, std::move(place_problem), std::move(place_input_data));
place_future->wait();

if (!place_input.isSuccessful())
if (!place_future->context->isSuccessful())
return false;

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = place_input.data_storage.getData(place_output_key).as<CompositeInstruction>();
auto ci = place_future->context->data_storage->getData(place_output_key).as<CompositeInstruction>();
tesseract_common::Toolpath toolpath = toToolpath(ci, *env_);
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
Expand Down
15 changes: 7 additions & 8 deletions tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
#include <tesseract_motion_planners/core/utils.h>
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
#include <tesseract_task_composer/core/task_composer_input.h>
#include <tesseract_task_composer/core/task_composer_context.h>
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
#include <tesseract_visualization/markers/toolpath_marker.h>
#include <trajopt_sco/osqp_interface.hpp>
Expand Down Expand Up @@ -230,31 +230,30 @@ bool PuzzlePieceAuxillaryAxesExample::run()
const std::string output_key = task->getOutputKeys().front();

// Create Task Input Data
TaskComposerDataStorage input_data;
input_data.setData(input_key, program);
auto input_data = std::make_unique<TaskComposerDataStorage>();
input_data->setData(input_key, program);

if (plotter_ != nullptr)
plotter_->waitForInput();

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

// Solve task
TaskComposerInput input(std::move(problem));
TaskComposerFuture::UPtr future = executor->run(*task, input);
TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data));
future->wait();

// Plot Process Trajectory
if (plotter_ != nullptr && plotter_->isConnected())
{
plotter_->waitForInput();
auto ci = input.data_storage.getData(output_key).as<CompositeInstruction>();
auto ci = future->context->data_storage->getData(output_key).as<CompositeInstruction>();
tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci);
auto state_solver = env_->getStateSolver();
plotter_->plotTrajectory(trajectory, *state_solver);
}

CONSOLE_BRIDGE_logInform("Final trajectory is collision free");
return input.isSuccessful();
return future->context->isSuccessful();
}
} // namespace tesseract_examples
Loading

0 comments on commit f216b4d

Please sign in to comment.