From a807ca19f8bbbd16f8b26c83da2f477ec409e47c Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 11 Oct 2024 21:17:57 -0500 Subject: [PATCH] Remove TesseractSupportResourceLocator --- tesseract_command_language/CHANGELOG.rst | 4 +- .../poly/instruction_poly.h | 1 - .../poly/waypoint_poly.h | 1 - .../src/cartesian_waypoint.cpp | 2 +- .../src/composite_instruction.cpp | 2 +- .../src/joint_waypoint.cpp | 2 +- .../src/move_instruction.cpp | 2 +- .../src/poly/cartesian_waypoint_poly.cpp | 8 +-- .../src/poly/instruction_poly.cpp | 7 +-- .../src/poly/joint_waypoint_poly.cpp | 8 +-- .../src/poly/move_instruction_poly.cpp | 8 +-- .../src/poly/state_waypoint_poly.cpp | 8 +-- .../src/poly/waypoint_poly.cpp | 7 +-- .../src/profile_dictionary.cpp | 2 +- .../src/set_analog_instruction.cpp | 2 +- .../src/set_tool_instruction.cpp | 2 +- .../src/state_waypoint.cpp | 2 +- .../src/timer_instruction.cpp | 2 +- .../src/wait_instruction.cpp | 2 +- tesseract_examples/CMakeLists.txt | 4 +- .../src/basic_cartesian_example.cpp | 10 ++++ .../src/basic_cartesian_example_node.cpp | 4 +- .../src/car_seat_example_node.cpp | 4 +- .../src/freespace_hybrid_example_node.cpp | 4 +- .../src/freespace_ompl_example_node.cpp | 4 +- .../src/glass_upright_example.cpp | 9 +++ .../src/glass_upright_example_node.cpp | 4 +- .../src/glass_upright_ompl_example_node.cpp | 4 +- .../src/online_planning_example_node.cpp | 4 +- .../src/pick_and_place_example_node.cpp | 4 +- ...zzle_piece_auxillary_axes_example_node.cpp | 4 +- .../src/puzzle_piece_example_node.cpp | 4 +- .../src/scene_graph_example_node.cpp | 4 +- tesseract_examples/test/CMakeLists.txt | 50 +++------------- .../test/basic_cartesian_example_unit.cpp | 6 +- .../test/car_seat_example_unit.cpp | 6 +- .../test/freespace_hybrid_example_unit.cpp | 6 +- .../test/glass_upright_example_unit.cpp | 6 +- .../test/pick_and_place_example_unit.cpp | 6 +- ...zzle_piece_auxillary_axes_example_unit.cpp | 6 +- .../test/puzzle_piece_example_unit.cpp | 6 +- .../test/scene_graph_example_unit.cpp | 4 +- tesseract_motion_planners/CHANGELOG.rst | 4 +- .../core/test/CMakeLists.txt | 2 - .../core/test/utils_test.cpp | 10 ++-- .../descartes/test/CMakeLists.txt | 2 - .../test/descartes_planner_tests.cpp | 10 ++-- .../examples/CMakeLists.txt | 4 -- .../examples/chain_example.cpp | 10 ++-- .../examples/freespace_example.cpp | 10 ++-- .../examples/raster_example.cpp | 10 ++-- .../ompl/test/CMakeLists.txt | 12 +--- .../ompl/test/ompl_planner_tests.cpp | 26 ++++---- .../simple/test/CMakeLists.txt | 25 ++------ ...ple_planner_fixed_size_assign_position.cpp | 10 ++-- ...imple_planner_fixed_size_interpolation.cpp | 10 ++-- .../test/simple_planner_lvs_interpolation.cpp | 10 ++-- .../trajopt/test/CMakeLists.txt | 8 +-- .../trajopt/test/trajopt_planner_tests.cpp | 10 ++-- tesseract_task_composer/core/CMakeLists.txt | 1 + .../tesseract_task_composer/core/fwd.h | 3 + .../core/task_composer_log.h | 59 +++++++++++++++++++ .../core/src/nodes/done_task.cpp | 2 +- .../core/src/nodes/error_task.cpp | 2 +- .../src/nodes/has_data_storage_entry_task.cpp | 2 +- .../core/src/nodes/remap_task.cpp | 2 +- .../core/src/nodes/start_task.cpp | 2 +- .../core/src/nodes/sync_task.cpp | 2 +- .../core/src/task_composer_context.cpp | 2 +- .../core/src/task_composer_data_storage.cpp | 2 +- .../core/src/task_composer_executor.cpp | 2 +- .../core/src/task_composer_graph.cpp | 2 +- .../core/src/task_composer_keys.cpp | 2 +- .../core/src/task_composer_log.cpp | 46 +++++++++++++++ .../core/src/task_composer_node.cpp | 2 +- .../core/src/task_composer_node_info.cpp | 6 +- .../core/src/task_composer_node_ports.cpp | 2 +- .../core/src/task_composer_pipeline.cpp | 2 +- .../core/src/task_composer_task.cpp | 2 +- .../core/src/test_suite/test_task.cpp | 2 +- .../examples/CMakeLists.txt | 10 +--- .../examples/task_composer_raster_example.cpp | 10 ++-- .../task_composer_trajopt_example.cpp | 10 ++-- .../nodes/continuous_contact_check_task.cpp | 2 +- .../src/nodes/discrete_contact_check_task.cpp | 2 +- .../src/nodes/fix_state_bounds_task.cpp | 2 +- .../src/nodes/fix_state_collision_task.cpp | 2 +- .../src/nodes/format_as_input_task.cpp | 2 +- .../src/nodes/format_as_result_task.cpp | 2 +- ...iterative_spline_parameterization_task.cpp | 2 +- .../planning/src/nodes/min_length_task.cpp | 2 +- .../src/nodes/process_planning_input_task.cpp | 2 +- .../src/nodes/profile_switch_task.cpp | 2 +- .../planning/src/nodes/raster_motion_task.cpp | 2 +- .../src/nodes/raster_only_motion_task.cpp | 2 +- .../ruckig_trajectory_smoothing_task.cpp | 2 +- .../time_optimal_parameterization_task.cpp | 2 +- .../src/nodes/update_end_state_task.cpp | 2 +- .../nodes/update_start_and_end_state_task.cpp | 2 +- .../src/nodes/update_start_state_task.cpp | 2 +- .../src/nodes/upsample_trajectory_task.cpp | 2 +- .../src/taskflow_task_composer_executor.cpp | 2 +- tesseract_task_composer/test/CMakeLists.txt | 3 - .../test/fix_state_bounds_task_unit.cpp | 10 ++-- .../test/fix_state_collision_task_unit.cpp | 10 ++-- .../tesseract_task_composer_core_unit.cpp | 11 ++++ .../tesseract_task_composer_planning_unit.cpp | 24 +++++--- 107 files changed, 391 insertions(+), 290 deletions(-) create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h create mode 100644 tesseract_task_composer/core/src/task_composer_log.cpp diff --git a/tesseract_command_language/CHANGELOG.rst b/tesseract_command_language/CHANGELOG.rst index 11292d869aa..70c7eb78b54 100644 --- a/tesseract_command_language/CHANGELOG.rst +++ b/tesseract_command_language/CHANGELOG.rst @@ -262,8 +262,8 @@ Changelog for package tesseract_command_language 0.9.5 (2022-03-31) ------------------ -* Update to leverage TesseractSupportResourceLocator (`#181 `_) - * Update to leverage TesseractSupportResourceLocator +* Update to leverage GeneralResourceLocator (`#181 `_) + * Update to leverage GeneralResourceLocator * Update CI docker tag to 0.9 * Fix windows CI build (`#178 `_) * Contributors: Levi Armstrong diff --git a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h index a5feb51e236..0cb3cf56d6c 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h @@ -205,7 +205,6 @@ struct InstructionPoly : InstructionPolyBase } // namespace tesseract_planning BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::detail_instruction::InstructionInterface) -BOOST_CLASS_EXPORT_KEY(tesseract_planning::detail_instruction::InstructionInterface) BOOST_CLASS_TRACKING(tesseract_planning::detail_instruction::InstructionInterface, boost::serialization::track_never) BOOST_CLASS_EXPORT_KEY(tesseract_planning::InstructionPolyBase) diff --git a/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h index d90d225ae34..6a4a26442bf 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h @@ -165,7 +165,6 @@ struct WaypointPoly : WaypointPolyBase } // namespace tesseract_planning BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::detail_waypoint::WaypointInterface) -BOOST_CLASS_EXPORT_KEY(tesseract_planning::detail_waypoint::WaypointInterface) BOOST_CLASS_TRACKING(tesseract_planning::detail_waypoint::WaypointInterface, boost::serialization::track_never) BOOST_CLASS_EXPORT_KEY(tesseract_planning::WaypointPolyBase) diff --git a/tesseract_command_language/src/cartesian_waypoint.cpp b/tesseract_command_language/src/cartesian_waypoint.cpp index 2eb19b26790..d0a1428d7bf 100644 --- a/tesseract_command_language/src/cartesian_waypoint.cpp +++ b/tesseract_command_language/src/cartesian_waypoint.cpp @@ -83,5 +83,5 @@ void tesseract_planning::CartesianWaypoint::serialize(Archive& ar, const unsigne #include -TESSERACT_CARTESIAN_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypoint) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypoint) +TESSERACT_CARTESIAN_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypoint) diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index e62e48c6dcd..c0b3e9dc08d 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -536,6 +536,6 @@ void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/ #include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CompositeInstruction) TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::CompositeInstruction) TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractPlanningCompositeInstruction) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CompositeInstruction) diff --git a/tesseract_command_language/src/joint_waypoint.cpp b/tesseract_command_language/src/joint_waypoint.cpp index 7327e4412ef..409d95df037 100644 --- a/tesseract_command_language/src/joint_waypoint.cpp +++ b/tesseract_command_language/src/joint_waypoint.cpp @@ -114,5 +114,5 @@ void JointWaypoint::serialize(Archive& ar, const unsigned int /*version*/) } } // namespace tesseract_planning -TESSERACT_JOINT_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::JointWaypoint) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypoint) +TESSERACT_JOINT_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::JointWaypoint) diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp index f00c01c6118..101297b7368 100644 --- a/tesseract_command_language/src/move_instruction.cpp +++ b/tesseract_command_language/src/move_instruction.cpp @@ -263,5 +263,5 @@ void MoveInstruction::serialize(Archive& ar, const unsigned int /*version*/) #include -TESSERACT_MOVE_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::MoveInstruction) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MoveInstruction) +TESSERACT_MOVE_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::MoveInstruction) diff --git a/tesseract_command_language/src/poly/cartesian_waypoint_poly.cpp b/tesseract_command_language/src/poly/cartesian_waypoint_poly.cpp index c10b8225060..9e4d00ae028 100644 --- a/tesseract_command_language/src/poly/cartesian_waypoint_poly.cpp +++ b/tesseract_command_language/src/poly/cartesian_waypoint_poly.cpp @@ -130,12 +130,12 @@ void tesseract_planning::CartesianWaypointPoly::serialize(Archive& ar, const uns ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_cartesian_waypoint::CartesianWaypointInterface) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointPolyBase) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointPoly) + BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_cartesian_waypoint::CartesianWaypointInterface) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypointPolyBase) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypointPoly) TESSERACT_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypointPoly) - -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_cartesian_waypoint::CartesianWaypointInterface) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointPolyBase) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointPoly) diff --git a/tesseract_command_language/src/poly/instruction_poly.cpp b/tesseract_command_language/src/poly/instruction_poly.cpp index 18a23d773d6..41e46e173c1 100644 --- a/tesseract_command_language/src/poly/instruction_poly.cpp +++ b/tesseract_command_language/src/poly/instruction_poly.cpp @@ -61,10 +61,9 @@ void tesseract_planning::InstructionPoly::serialize(Archive& ar, const unsigned ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_instruction::InstructionInterface) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::InstructionPolyBase) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::InstructionPoly) - TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_instruction::InstructionInterface) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::InstructionPolyBase) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::InstructionPoly) + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::InstructionPolyBase) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::InstructionPoly) diff --git a/tesseract_command_language/src/poly/joint_waypoint_poly.cpp b/tesseract_command_language/src/poly/joint_waypoint_poly.cpp index 90af5b0ea4b..3449eab303b 100644 --- a/tesseract_command_language/src/poly/joint_waypoint_poly.cpp +++ b/tesseract_command_language/src/poly/joint_waypoint_poly.cpp @@ -122,12 +122,12 @@ void tesseract_planning::JointWaypointPoly::serialize(Archive& ar, const unsigne ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_joint_waypoint::JointWaypointInterface) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointPolyBase) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointPoly) + BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_joint_waypoint::JointWaypointInterface) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::JointWaypointPolyBase) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::JointWaypointPoly) TESSERACT_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::JointWaypointPoly) - -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_joint_waypoint::JointWaypointInterface) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointPolyBase) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointPoly) diff --git a/tesseract_command_language/src/poly/move_instruction_poly.cpp b/tesseract_command_language/src/poly/move_instruction_poly.cpp index 786d51c47b9..f1918ce42a2 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -171,12 +171,12 @@ void tesseract_planning::MoveInstructionPoly::serialize(Archive& ar, const unsig ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_move_instruction::MoveInstructionInterface) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MoveInstructionPolyBase) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MoveInstructionPoly) + BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_move_instruction::MoveInstructionInterface) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MoveInstructionPolyBase) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MoveInstructionPoly) TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::MoveInstructionPoly) - -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_move_instruction::MoveInstructionInterface) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MoveInstructionPolyBase) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MoveInstructionPoly) diff --git a/tesseract_command_language/src/poly/state_waypoint_poly.cpp b/tesseract_command_language/src/poly/state_waypoint_poly.cpp index 1b2b0b30ce4..2691bb19c06 100644 --- a/tesseract_command_language/src/poly/state_waypoint_poly.cpp +++ b/tesseract_command_language/src/poly/state_waypoint_poly.cpp @@ -101,12 +101,12 @@ void tesseract_planning::StateWaypointPoly::serialize(Archive& ar, const unsigne ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_state_waypoint::StateWaypointInterface) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StateWaypointPolyBase) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StateWaypointPoly) + BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_state_waypoint::StateWaypointInterface) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::StateWaypointPolyBase) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::StateWaypointPoly) TESSERACT_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::StateWaypointPoly) - -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_state_waypoint::StateWaypointInterface) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StateWaypointPolyBase) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StateWaypointPoly) diff --git a/tesseract_command_language/src/poly/waypoint_poly.cpp b/tesseract_command_language/src/poly/waypoint_poly.cpp index aec0c6ad0c1..72d90c06696 100644 --- a/tesseract_command_language/src/poly/waypoint_poly.cpp +++ b/tesseract_command_language/src/poly/waypoint_poly.cpp @@ -43,10 +43,9 @@ void tesseract_planning::WaypointPoly::serialize(Archive& ar, const unsigned int ar& boost::serialization::make_nvp("base", boost::serialization::base_object(*this)); } -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::detail_waypoint::WaypointInterface) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::WaypointPolyBase) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::WaypointPoly) - TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::detail_waypoint::WaypointInterface) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::WaypointPolyBase) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::WaypointPoly) + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::WaypointPolyBase) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::WaypointPoly) diff --git a/tesseract_command_language/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index 00a2c57b7e6..50657397a93 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -43,6 +43,6 @@ void ProfileDictionary::serialize(Archive& /*ar*/, const unsigned int /*version* } // namespace tesseract_planning #include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProfileDictionary) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProfileDictionary) TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractPlanningProfileDictionarySharedPtr) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProfileDictionary) diff --git a/tesseract_command_language/src/set_analog_instruction.cpp b/tesseract_command_language/src/set_analog_instruction.cpp index 35355d06e54..59a1ff9ca32 100644 --- a/tesseract_command_language/src/set_analog_instruction.cpp +++ b/tesseract_command_language/src/set_analog_instruction.cpp @@ -101,5 +101,5 @@ void SetAnalogInstruction::serialize(Archive& ar, const unsigned int /*version*/ } // namespace tesseract_planning #include -TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::SetAnalogInstruction) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SetAnalogInstruction) +TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::SetAnalogInstruction) diff --git a/tesseract_command_language/src/set_tool_instruction.cpp b/tesseract_command_language/src/set_tool_instruction.cpp index af361bd2b3a..225c6708aef 100644 --- a/tesseract_command_language/src/set_tool_instruction.cpp +++ b/tesseract_command_language/src/set_tool_instruction.cpp @@ -89,5 +89,5 @@ void SetToolInstruction::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::SetToolInstruction) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SetToolInstruction) +TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::SetToolInstruction) diff --git a/tesseract_command_language/src/state_waypoint.cpp b/tesseract_command_language/src/state_waypoint.cpp index 26374d89a74..db39c88a945 100644 --- a/tesseract_command_language/src/state_waypoint.cpp +++ b/tesseract_command_language/src/state_waypoint.cpp @@ -133,5 +133,5 @@ void StateWaypoint::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -TESSERACT_STATE_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::StateWaypoint) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StateWaypoint) +TESSERACT_STATE_WAYPOINT_EXPORT_IMPLEMENT(tesseract_planning::StateWaypoint) diff --git a/tesseract_command_language/src/timer_instruction.cpp b/tesseract_command_language/src/timer_instruction.cpp index e1d9d1fe62f..464e7d3df83 100644 --- a/tesseract_command_language/src/timer_instruction.cpp +++ b/tesseract_command_language/src/timer_instruction.cpp @@ -107,5 +107,5 @@ void TimerInstruction::serialize(Archive& ar, const unsigned int /*version*/) } } // namespace tesseract_planning -TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::TimerInstruction) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TimerInstruction) +TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::TimerInstruction) diff --git a/tesseract_command_language/src/wait_instruction.cpp b/tesseract_command_language/src/wait_instruction.cpp index 79ebf53d1bb..0a63265c5e7 100644 --- a/tesseract_command_language/src/wait_instruction.cpp +++ b/tesseract_command_language/src/wait_instruction.cpp @@ -108,5 +108,5 @@ void WaitInstruction::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning #include -TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::WaitInstruction) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::WaitInstruction) +TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(tesseract_planning::WaitInstruction) diff --git a/tesseract_examples/CMakeLists.txt b/tesseract_examples/CMakeLists.txt index 8ee89949b2a..b4f18426709 100644 --- a/tesseract_examples/CMakeLists.txt +++ b/tesseract_examples/CMakeLists.txt @@ -9,7 +9,6 @@ find_package(tesseract_environment REQUIRED) find_package(tesseract_command_language REQUIRED) find_package(tesseract_task_composer REQUIRED) find_package(tesseract_common REQUIRED) -find_package(tesseract_support REQUIRED) find_package(trajopt_sqp REQUIRED) find_package(trajopt_ifopt REQUIRED) find_package( @@ -106,7 +105,7 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) macro(add_example example_name example_file) add_executable(${example_name} ${example_file}) - target_link_libraries(${example_name} ${PROJECT_NAME} tesseract::tesseract_support) + target_link_libraries(${example_name} ${PROJECT_NAME}) target_compile_options(${example_name} PRIVATE ${TESSERACT_COMPILE_OPTIONS}) target_clang_tidy(${example_name} ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) target_cxx_version(${example_name} PUBLIC VERSION ${TESSERACT_CXX_VERSION}) @@ -156,7 +155,6 @@ configure_package( tesseract_task_composer tesseract_collision tesseract_common - tesseract_support trajopt_sqp trajopt_ifopt "PCL REQUIRED COMPONENTS core features filters io segmentation surface") diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 2ff9517c4f0..e768bdab0ca 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -69,6 +69,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -263,6 +264,9 @@ bool BasicCartesianExample::run() profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } + // Create log + tesseract_planning::TaskComposerLog log; + // Create task const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); @@ -274,6 +278,8 @@ bool BasicCartesianExample::run() data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); + // Store initial data in log + log.initial_data = *data; if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); @@ -286,6 +292,10 @@ bool BasicCartesianExample::run() stopwatch.stop(); CONSOLE_BRIDGE_logInform("Planning took %f seconds.", stopwatch.elapsedSeconds()); + // Store context in log and save to temp + log.context = future->context; + tesseract_common::Serialization::toArchiveFileXML(log, tesseract_common::getTempPath() + "basic_cartesian_log"); + // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { diff --git a/tesseract_examples/src/basic_cartesian_example_node.cpp b/tesseract_examples/src/basic_cartesian_example_node.cpp index 24b16963410..60a9430cfa7 100644 --- a/tesseract_examples/src/basic_cartesian_example_node.cpp +++ b/tesseract_examples/src/basic_cartesian_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/car_seat_example_node.cpp b/tesseract_examples/src/car_seat_example_node.cpp index cf76abfdf75..782bb75b0a1 100644 --- a/tesseract_examples/src/car_seat_example_node.cpp +++ b/tesseract_examples/src/car_seat_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/car_seat_demo.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/freespace_hybrid_example_node.cpp b/tesseract_examples/src/freespace_hybrid_example_node.cpp index 5075e81097c..adf41fc5b1a 100644 --- a/tesseract_examples/src/freespace_hybrid_example_node.cpp +++ b/tesseract_examples/src/freespace_hybrid_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/freespace_ompl_example_node.cpp b/tesseract_examples/src/freespace_ompl_example_node.cpp index a9dd6c64be9..a857c95c2ae 100644 --- a/tesseract_examples/src/freespace_ompl_example_node.cpp +++ b/tesseract_examples/src/freespace_ompl_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index fe1dbb2714f..0d16221245b 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -57,6 +57,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -258,12 +259,16 @@ bool GlassUprightExample::run() TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().get("program"); + // Create log + tesseract_planning::TaskComposerLog log; + // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); + log.initial_data = *data; if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); @@ -276,6 +281,10 @@ bool GlassUprightExample::run() stopwatch.stop(); CONSOLE_BRIDGE_logInform("Planning took %f seconds.", stopwatch.elapsedSeconds()); + // Store context in log and save to temp + log.context = future->context; + tesseract_common::Serialization::toArchiveFileXML(log, tesseract_common::getTempPath() + "glass_upright_example"); + // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { diff --git a/tesseract_examples/src/glass_upright_example_node.cpp b/tesseract_examples/src/glass_upright_example_node.cpp index 39bf785352d..f3be4f959be 100644 --- a/tesseract_examples/src/glass_upright_example_node.cpp +++ b/tesseract_examples/src/glass_upright_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/glass_upright_ompl_example_node.cpp b/tesseract_examples/src/glass_upright_ompl_example_node.cpp index bd6c8d45c30..d9715c1a0c9 100644 --- a/tesseract_examples/src/glass_upright_ompl_example_node.cpp +++ b/tesseract_examples/src/glass_upright_ompl_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int argc, char** argv) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/online_planning_example_node.cpp b/tesseract_examples/src/online_planning_example_node.cpp index cb5cb7cc1d1..2897ef52cfe 100644 --- a/tesseract_examples/src/online_planning_example_node.cpp +++ b/tesseract_examples/src/online_planning_example_node.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -40,7 +40,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/online_planning_example.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/pick_and_place_example_node.cpp b/tesseract_examples/src/pick_and_place_example_node.cpp index 84af4bf5ce3..06e447a2132 100644 --- a/tesseract_examples/src/pick_and_place_example_node.cpp +++ b/tesseract_examples/src/pick_and_place_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example_node.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example_node.cpp index b80d5d1368d..bd97c98489d 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example_node.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/puzzle_piece_example_node.cpp b/tesseract_examples/src/puzzle_piece_example_node.cpp index 2ecaca9d9ac..971d150ce67 100644 --- a/tesseract_examples/src/puzzle_piece_example_node.cpp +++ b/tesseract_examples/src/puzzle_piece_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/src/scene_graph_example_node.cpp b/tesseract_examples/src/scene_graph_example_node.cpp index e17e664694e..fb73656efea 100644 --- a/tesseract_examples/src/scene_graph_example_node.cpp +++ b/tesseract_examples/src/scene_graph_example_node.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -35,7 +35,7 @@ using namespace tesseract_environment; int main(int /*argc*/, char** /*argv*/) { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/scene_graph_example.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/CMakeLists.txt b/tesseract_examples/test/CMakeLists.txt index f5f9c234d4f..8d3c81c72f6 100644 --- a/tesseract_examples/test/CMakeLists.txt +++ b/tesseract_examples/test/CMakeLists.txt @@ -31,12 +31,7 @@ endif() include(GoogleTest) add_executable(${PROJECT_NAME}_basic_cartesian_example_unit basic_cartesian_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_basic_cartesian_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_basic_cartesian_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_basic_cartesian_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_basic_cartesian_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -47,12 +42,7 @@ add_dependencies(${PROJECT_NAME}_basic_cartesian_example_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_basic_cartesian_example_unit) add_executable(${PROJECT_NAME}_car_seat_example_unit car_seat_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_car_seat_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_car_seat_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_car_seat_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_car_seat_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -63,12 +53,7 @@ add_dependencies(${PROJECT_NAME}_car_seat_example_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_car_seat_example_unit) add_executable(${PROJECT_NAME}_glass_upright_example_unit glass_upright_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_glass_upright_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_glass_upright_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_glass_upright_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_glass_upright_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -79,12 +64,7 @@ add_dependencies(${PROJECT_NAME}_glass_upright_example_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_glass_upright_example_unit) add_executable(${PROJECT_NAME}_pick_and_place_example_unit pick_and_place_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_pick_and_place_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_pick_and_place_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_pick_and_place_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_pick_and_place_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -95,12 +75,8 @@ add_dependencies(${PROJECT_NAME}_pick_and_place_example_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_pick_and_place_example_unit) add_executable(${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit puzzle_piece_auxillary_axes_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit @@ -112,12 +88,7 @@ add_dependencies(${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit ${PROJ add_dependencies(run_tests ${PROJECT_NAME}_puzzle_piece_auxillary_axes_example_unit) add_executable(${PROJECT_NAME}_puzzle_piece_example_unit puzzle_piece_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_puzzle_piece_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_puzzle_piece_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_puzzle_piece_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_puzzle_piece_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -128,12 +99,7 @@ add_dependencies(${PROJECT_NAME}_puzzle_piece_example_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_puzzle_piece_example_unit) add_executable(${PROJECT_NAME}_scene_graph_example_unit scene_graph_example_unit.cpp) -target_link_libraries( - ${PROJECT_NAME}_scene_graph_example_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME} - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_scene_graph_example_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) target_compile_options(${PROJECT_NAME}_scene_graph_example_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_scene_graph_example_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_examples/test/basic_cartesian_example_unit.cpp b/tesseract_examples/test/basic_cartesian_example_unit.cpp index 299092016bb..d18b1806115 100644 --- a/tesseract_examples/test/basic_cartesian_example_unit.cpp +++ b/tesseract_examples/test/basic_cartesian_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, BasicCartesianTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, BasicCartesianTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, BasicCartesianTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/car_seat_example_unit.cpp b/tesseract_examples/test/car_seat_example_unit.cpp index 211a25380fe..00f8a06a3c8 100644 --- a/tesseract_examples/test/car_seat_example_unit.cpp +++ b/tesseract_examples/test/car_seat_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, CarSeatCppTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/car_seat_demo.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, CarSeatCppTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, CarSeatCppTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/car_seat_demo.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/freespace_hybrid_example_unit.cpp b/tesseract_examples/test/freespace_hybrid_example_unit.cpp index ea352481a76..46e4ecb5798 100644 --- a/tesseract_examples/test/freespace_hybrid_example_unit.cpp +++ b/tesseract_examples/test/freespace_hybrid_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, FreespaceHybridTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, FreespaceHybridTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, FreespaceHybridTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/glass_upright_example_unit.cpp b/tesseract_examples/test/glass_upright_example_unit.cpp index 8f7fec76b4d..fd3bd95a54e 100644 --- a/tesseract_examples/test/glass_upright_example_unit.cpp +++ b/tesseract_examples/test/glass_upright_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, GlassUprightTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, GlassUprightTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, GlassUprightTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp index faa7400ddb0..aaee3c7b841 100644 --- a/tesseract_examples/test/pick_and_place_example_unit.cpp +++ b/tesseract_examples/test/pick_and_place_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp index a4ce03fbc8f..152e5638fed 100644 --- a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLIN TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/puzzle_piece_example_unit.cpp b/tesseract_examples/test/puzzle_piece_example_unit.cpp index 35240025749..8691d404f9b 100644 --- a/tesseract_examples/test/puzzle_piece_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_example_unit.cpp @@ -6,7 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -14,7 +14,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, PuzzlePieceCppTrajOptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = @@ -29,7 +29,7 @@ TEST(TesseractExamples, PuzzlePieceCppTrajOptExampleUnit) // NOLINT TEST(TesseractExamples, PuzzlePieceCppTrajOptIfoptExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_examples/test/scene_graph_example_unit.cpp b/tesseract_examples/test/scene_graph_example_unit.cpp index f25d337366d..a93991cc957 100644 --- a/tesseract_examples/test/scene_graph_example_unit.cpp +++ b/tesseract_examples/test/scene_graph_example_unit.cpp @@ -32,7 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_examples; using namespace tesseract_common; @@ -40,7 +40,7 @@ using namespace tesseract_environment; TEST(TesseractExamples, SceneGraphExampleUnit) // NOLINT { - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/scene_graph_example.urdf")->getFilePath(); tesseract_common::fs::path srdf_path = diff --git a/tesseract_motion_planners/CHANGELOG.rst b/tesseract_motion_planners/CHANGELOG.rst index 7698aca74f6..e5662c367b2 100644 --- a/tesseract_motion_planners/CHANGELOG.rst +++ b/tesseract_motion_planners/CHANGELOG.rst @@ -278,8 +278,8 @@ Changelog for package tesseract_motion_planners 0.9.5 (2022-03-31) ------------------ -* Update to leverage TesseractSupportResourceLocator (`#181 `_) - * Update to leverage TesseractSupportResourceLocator +* Update to leverage GeneralResourceLocator (`#181 `_) + * Update to leverage GeneralResourceLocator * Update CI docker tag to 0.9 * Contributors: Levi Armstrong diff --git a/tesseract_motion_planners/core/test/CMakeLists.txt b/tesseract_motion_planners/core/test/CMakeLists.txt index dd8ac152279..47432d49986 100644 --- a/tesseract_motion_planners/core/test/CMakeLists.txt +++ b/tesseract_motion_planners/core/test/CMakeLists.txt @@ -1,4 +1,3 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_command_language REQUIRED) # Profile Dictionary Tests @@ -25,7 +24,6 @@ target_link_libraries( ${PROJECT_NAME}_utils_unit PRIVATE GTest::GTest GTest::Main - tesseract::tesseract_support tesseract::tesseract_command_language ${PROJECT_NAME}_core ${PROJECT_NAME}_simple) diff --git a/tesseract_motion_planners/core/test/utils_test.cpp b/tesseract_motion_planners/core/test/utils_test.cpp index 36e771454b3..45a664f8c3b 100644 --- a/tesseract_motion_planners/core/test/utils_test.cpp +++ b/tesseract_motion_planners/core/test/utils_test.cpp @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; using namespace tesseract_environment; @@ -55,10 +55,12 @@ class TesseractPlanningUtilsUnit : public ::testing::Test void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; } diff --git a/tesseract_motion_planners/descartes/test/CMakeLists.txt b/tesseract_motion_planners/descartes/test/CMakeLists.txt index aed8a549f84..07fa93b8229 100644 --- a/tesseract_motion_planners/descartes/test/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/test/CMakeLists.txt @@ -1,4 +1,3 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_command_language REQUIRED) find_package(tesseract_kinematics REQUIRED COMPONENTS opw) @@ -7,7 +6,6 @@ target_link_libraries( ${PROJECT_NAME}_descartes_unit PRIVATE GTest::GTest GTest::Main - tesseract::tesseract_support tesseract::tesseract_kinematics_opw ${PROJECT_NAME}_descartes) target_compile_options(${PROJECT_NAME}_descartes_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index 76f151be9b6..b8dc02bd2f1 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include using namespace tesseract_environment; using namespace tesseract_scene_graph; @@ -73,10 +73,12 @@ class TesseractPlanningDescartesUnit : public ::testing::Test void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_motion_planners/examples/CMakeLists.txt b/tesseract_motion_planners/examples/CMakeLists.txt index 36c4941dfce..84af211528c 100644 --- a/tesseract_motion_planners/examples/CMakeLists.txt +++ b/tesseract_motion_planners/examples/CMakeLists.txt @@ -1,4 +1,3 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_visualization REQUIRED) if(TESSERACT_BUILD_SIMPLE AND TESSERACT_BUILD_DESCARTES AND TESSERACT_BUILD_TRAJOPT) @@ -10,7 +9,6 @@ if(TESSERACT_BUILD_SIMPLE AND TESSERACT_BUILD_DESCARTES AND TESSERACT_BUILD_TRAJ ${PROJECT_NAME}_trajopt ${PROJECT_NAME}_simple tesseract::tesseract_state_solver_ofkt - tesseract::tesseract_support tesseract::tesseract_visualization) target_compile_options(${PROJECT_NAME}_chain_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -29,7 +27,6 @@ if(TESSERACT_BUILD_SIMPLE AND TESSERACT_BUILD_DESCARTES AND TESSERACT_BUILD_TRAJ ${PROJECT_NAME}_trajopt ${PROJECT_NAME}_simple tesseract::tesseract_state_solver_ofkt - tesseract::tesseract_support tesseract::tesseract_visualization) target_compile_options(${PROJECT_NAME}_raster_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) @@ -48,7 +45,6 @@ if(TESSERACT_BUILD_SIMPLE AND TESSERACT_BUILD_OMPL AND TESSERACT_BUILD_TRAJOPT) ${PROJECT_NAME}_trajopt ${PROJECT_NAME}_simple tesseract::tesseract_state_solver_ofkt - tesseract::tesseract_support tesseract::tesseract_visualization) target_compile_options(${PROJECT_NAME}_freespace_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index c7d5ffc4b1e..019612e281c 100644 --- a/tesseract_motion_planners/examples/chain_example.cpp +++ b/tesseract_motion_planners/examples/chain_example.cpp @@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; @@ -65,10 +65,12 @@ int main(int /*argc*/, char** /*argv*/) try { // Setup - auto locator = std::make_shared(); + auto locator = std::make_shared(); auto env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); env->init(urdf_path, srdf_path, locator); // Dynamically load ignition visualizer if exist diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 3b1c16e250b..c235fb9a53e 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -50,7 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; @@ -62,10 +62,12 @@ int main(int /*argc*/, char** /*argv*/) try { // Setup - auto locator = std::make_shared(); + auto locator = std::make_shared(); auto env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); env->init(urdf_path, srdf_path, locator); // Dynamically load ignition visualizer if exist diff --git a/tesseract_motion_planners/examples/raster_example.cpp b/tesseract_motion_planners/examples/raster_example.cpp index e8b434e3d4d..0554bc7565f 100644 --- a/tesseract_motion_planners/examples/raster_example.cpp +++ b/tesseract_motion_planners/examples/raster_example.cpp @@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; @@ -65,10 +65,12 @@ int main(int /*argc*/, char** /*argv*/) try { // Setup - auto locator = std::make_shared(); + auto locator = std::make_shared(); auto env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); env->init(urdf_path, srdf_path, locator); // Dynamically load ignition visualizer if exist diff --git a/tesseract_motion_planners/ompl/test/CMakeLists.txt b/tesseract_motion_planners/ompl/test/CMakeLists.txt index 12fd10d1ac7..1e79816e14a 100644 --- a/tesseract_motion_planners/ompl/test/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/test/CMakeLists.txt @@ -1,13 +1,7 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_command_language REQUIRED) add_executable(${PROJECT_NAME}_ompl_unit ompl_planner_tests.cpp) -target_link_libraries( - ${PROJECT_NAME}_ompl_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME}_ompl - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_ompl_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_ompl) target_compile_options(${PROJECT_NAME}_ompl_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_ompl_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -26,8 +20,8 @@ add_dependencies(run_tests ${PROJECT_NAME}_ompl_unit) # OMPL Constrained Planning Test/Example Program if(NOT OMPL_VERSION VERSION_LESS "1.4.0") # add_executable(${PROJECT_NAME}_ompl_constrained_unit ompl_constrained_planner_tests.cpp) # target_link_libraries(${PROJECT_NAME}_ompl_constrained_unit PRIVATE Boost::boost Boost::serialization Boost::system -# Boost::filesystem Boost::program_options GTest::GTest GTest::Main ${PROJECT_NAME}_ompl tesseract::tesseract_support -# trajopt::trajopt) tesseract_target_compile_options(${PROJECT_NAME}_ompl_constrained_unit PRIVATE) +# Boost::filesystem Boost::program_options GTest::GTest GTest::Main ${PROJECT_NAME}_ompl trajopt::trajopt) +# tesseract_target_compile_options(${PROJECT_NAME}_ompl_constrained_unit PRIVATE) # tesseract_clang_tidy(${PROJECT_NAME}_ompl_constrained_unit) # tesseract_code_coverage(${PROJECT_NAME}_ompl_constrained_unit ALL EXCLUDE ${COVERAGE_EXCLUDE}) # tesseract_gtest_discover_tests(${PROJECT_NAME}_ompl_constrained_unit) diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 67bc83c6c1e..09e771152b2 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -76,7 +76,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include @@ -180,10 +180,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT << " vs. " << SEED; // Step 1: Load scene and srdf - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); tesseract_common::ManipulatorInfo manip; @@ -343,10 +345,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT << " vs. " << SEED; // Step 1: Load scene and srdf - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); // Set manipulator @@ -436,10 +440,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT << " vs. " << SEED; // Step 1: Load scene and srdf - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); // Set manipulator diff --git a/tesseract_motion_planners/simple/test/CMakeLists.txt b/tesseract_motion_planners/simple/test/CMakeLists.txt index 60b1c59739b..f489def804d 100644 --- a/tesseract_motion_planners/simple/test/CMakeLists.txt +++ b/tesseract_motion_planners/simple/test/CMakeLists.txt @@ -1,14 +1,9 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_command_language REQUIRED) # SimplePlanner Tests add_executable(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit simple_planner_fixed_size_interpolation.cpp) -target_link_libraries( - ${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit - PRIVATE GTest::GTest - GTest::Main - tesseract::tesseract_support - ${PROJECT_NAME}_simple) +target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit @@ -28,12 +23,8 @@ add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_interpolati add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit simple_planner_fixed_size_assign_position.cpp) -target_link_libraries( - ${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME}_simple - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit @@ -52,12 +43,8 @@ add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit) add_executable(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit simple_planner_lvs_interpolation.cpp) -target_link_libraries( - ${PROJECT_NAME}_simple_planner_lvs_interpolation_unit - PRIVATE GTest::GTest - GTest::Main - ${PROJECT_NAME}_simple - tesseract::tesseract_support) +target_link_libraries(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) target_compile_options(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 7364a9dbfb7..85f4d6301d1 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_environment; using namespace tesseract_planning; @@ -52,10 +52,12 @@ class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testi void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp index a61f58db96b..2ff99cffb92 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp @@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_environment; using namespace tesseract_planning; @@ -52,10 +52,12 @@ class TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : public ::testin void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp index cb96245b1dc..49b834f42f6 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp @@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_environment; using namespace tesseract_planning; @@ -52,10 +52,12 @@ class TesseractPlanningSimplePlannerLVSInterpolationUnit : public ::testing::Tes void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_motion_planners/trajopt/test/CMakeLists.txt b/tesseract_motion_planners/trajopt/test/CMakeLists.txt index 8e4fff727e3..5c0aa0e4e5f 100644 --- a/tesseract_motion_planners/trajopt/test/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt/test/CMakeLists.txt @@ -1,13 +1,7 @@ -find_package(tesseract_support REQUIRED) find_package(tesseract_command_language REQUIRED) add_executable(${PROJECT_NAME}_trajopt_unit trajopt_planner_tests.cpp) -target_link_libraries( - ${PROJECT_NAME}_trajopt_unit - PRIVATE GTest::GTest - GTest::Main - tesseract::tesseract_support - ${PROJECT_NAME}_trajopt) +target_link_libraries(${PROJECT_NAME}_trajopt_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_trajopt) target_compile_options(${PROJECT_NAME}_trajopt_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_trajopt_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 18219cdfece..446fa1ec105 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -52,7 +52,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include const int NUM_STEPS = 7; @@ -72,10 +72,12 @@ class TesseractPlanningTrajoptUnit : public ::testing::Test void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; manip.tcp_frame = "tool0"; diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index ca5b5c1b182..70666345217 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -9,6 +9,7 @@ add_library( src/task_composer_future.cpp src/task_composer_graph.cpp src/task_composer_keys.cpp + src/task_composer_log.cpp src/task_composer_node_info.cpp src/task_composer_node_ports.cpp src/task_composer_node.cpp diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/fwd.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/fwd.h index 71b38c2d56b..c1d678f3be4 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/fwd.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/fwd.h @@ -21,6 +21,9 @@ class TaskComposerGraph; // task_composer_keys.h class TaskComposerKeys; +// task_composer_log.h +class TaskComposerLog; + // task_composer_node_info.h class TaskComposerNodeInfo; class TaskComposerNodeInfoContainer; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h new file mode 100644 index 00000000000..376eef4ee7f --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h @@ -0,0 +1,59 @@ +/** + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_TASK_COMPOSER_TASK_COMPOSER_LOG_H +#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_LOG_H + +#include +#include +#include +#include + +namespace boost::serialization +{ +class access; +} + +namespace tesseract_planning +{ +class TaskComposerContext; + +class TaskComposerLog +{ +public: + virtual ~TaskComposerLog() = default; + + TaskComposerDataStorage initial_data; + std::shared_ptr context; + std::string description; + + bool operator==(const TaskComposerLog& rhs) const; + bool operator!=(const TaskComposerLog& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TaskComposerLog) +TESSERACT_CLASS_EXTENSION(tesseract_planning::TaskComposerLog, ".tclx", ".tclb") + +#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_LOG_H diff --git a/tesseract_task_composer/core/src/nodes/done_task.cpp b/tesseract_task_composer/core/src/nodes/done_task.cpp index d0f57449a51..ffceb8a426c 100644 --- a/tesseract_task_composer/core/src/nodes/done_task.cpp +++ b/tesseract_task_composer/core/src/nodes/done_task.cpp @@ -69,5 +69,5 @@ void DoneTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DoneTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DoneTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DoneTask) diff --git a/tesseract_task_composer/core/src/nodes/error_task.cpp b/tesseract_task_composer/core/src/nodes/error_task.cpp index 7bca15d2f34..3f60bb267b4 100644 --- a/tesseract_task_composer/core/src/nodes/error_task.cpp +++ b/tesseract_task_composer/core/src/nodes/error_task.cpp @@ -69,5 +69,5 @@ void ErrorTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ErrorTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ErrorTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ErrorTask) diff --git a/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp b/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp index e3b59440a74..5fbe152dd8e 100644 --- a/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp +++ b/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp @@ -79,5 +79,5 @@ void HasDataStorageEntryTask::serialize(Archive& ar, const unsigned int /*versio } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::HasDataStorageEntryTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::HasDataStorageEntryTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::HasDataStorageEntryTask) diff --git a/tesseract_task_composer/core/src/nodes/remap_task.cpp b/tesseract_task_composer/core/src/nodes/remap_task.cpp index 15e033eec95..b2aa60f5252 100644 --- a/tesseract_task_composer/core/src/nodes/remap_task.cpp +++ b/tesseract_task_composer/core/src/nodes/remap_task.cpp @@ -125,5 +125,5 @@ void RemapTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RemapTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RemapTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RemapTask) diff --git a/tesseract_task_composer/core/src/nodes/start_task.cpp b/tesseract_task_composer/core/src/nodes/start_task.cpp index 2e8dcae814f..610600f0b24 100644 --- a/tesseract_task_composer/core/src/nodes/start_task.cpp +++ b/tesseract_task_composer/core/src/nodes/start_task.cpp @@ -65,5 +65,5 @@ void StartTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::StartTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::StartTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::StartTask) diff --git a/tesseract_task_composer/core/src/nodes/sync_task.cpp b/tesseract_task_composer/core/src/nodes/sync_task.cpp index 1e64da294aa..436462ff84c 100644 --- a/tesseract_task_composer/core/src/nodes/sync_task.cpp +++ b/tesseract_task_composer/core/src/nodes/sync_task.cpp @@ -65,5 +65,5 @@ void SyncTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SyncTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SyncTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SyncTask) diff --git a/tesseract_task_composer/core/src/task_composer_context.cpp b/tesseract_task_composer/core/src/task_composer_context.cpp index f34d27aeb6e..f0c7195f726 100644 --- a/tesseract_task_composer/core/src/task_composer_context.cpp +++ b/tesseract_task_composer/core/src/task_composer_context.cpp @@ -93,5 +93,5 @@ void TaskComposerContext::serialize(Archive& ar, const unsigned int /*version*/) } } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerContext) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerContext) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerContext) diff --git a/tesseract_task_composer/core/src/task_composer_data_storage.cpp b/tesseract_task_composer/core/src/task_composer_data_storage.cpp index b6b9d5640d9..0533b0a8731 100644 --- a/tesseract_task_composer/core/src/task_composer_data_storage.cpp +++ b/tesseract_task_composer/core/src/task_composer_data_storage.cpp @@ -192,5 +192,5 @@ void TaskComposerDataStorage::serialize(Archive& ar, const unsigned int /*versio } } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerDataStorage) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerDataStorage) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerDataStorage) diff --git a/tesseract_task_composer/core/src/task_composer_executor.cpp b/tesseract_task_composer/core/src/task_composer_executor.cpp index 4fb51fb5e9c..990580c0e88 100644 --- a/tesseract_task_composer/core/src/task_composer_executor.cpp +++ b/tesseract_task_composer/core/src/task_composer_executor.cpp @@ -65,5 +65,5 @@ void TaskComposerExecutor::serialize(Archive& ar, const unsigned int /*version*/ } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerExecutor) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerExecutor) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerExecutor) diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index 8b01d23a5be..945fc328041 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -568,5 +568,5 @@ void TaskComposerGraph::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerGraph) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerGraph) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerGraph) diff --git a/tesseract_task_composer/core/src/task_composer_keys.cpp b/tesseract_task_composer/core/src/task_composer_keys.cpp index ac7c1aad601..e0573c6fa26 100644 --- a/tesseract_task_composer/core/src/task_composer_keys.cpp +++ b/tesseract_task_composer/core/src/task_composer_keys.cpp @@ -113,5 +113,5 @@ std::ostream& operator<<(std::ostream& os, const TaskComposerKeys& keys) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerKeys) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerKeys) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerKeys) diff --git a/tesseract_task_composer/core/src/task_composer_log.cpp b/tesseract_task_composer/core/src/task_composer_log.cpp new file mode 100644 index 00000000000..efe8232fc83 --- /dev/null +++ b/tesseract_task_composer/core/src/task_composer_log.cpp @@ -0,0 +1,46 @@ +/** + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +bool TaskComposerLog::operator==(const TaskComposerLog& rhs) const +{ + return ((initial_data == rhs.initial_data) && (*context == *rhs.context) && (description == rhs.description)); +} + +// LCOV_EXCL_START +bool TaskComposerLog::operator!=(const TaskComposerLog& rhs) const { return !operator==(rhs); } +// LCOV_EXCL_STOP + +template +void TaskComposerLog::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(initial_data); + ar& BOOST_SERIALIZATION_NVP(context); + ar& BOOST_SERIALIZATION_NVP(description); +} + +} // namespace tesseract_planning + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerLog) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerLog) diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 3328b719937..17693ec0267 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -618,5 +618,5 @@ void TaskComposerNode::setData(TaskComposerDataStorage& data_storage, } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNode) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerNode) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNode) diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp index c92cbc7360f..2a859c9f09e 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -331,8 +331,8 @@ void TaskComposerNodeInfoContainer::serialize(Archive& ar, const unsigned int /* } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodeInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodeInfoContainer) - TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerNodeInfo) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerNodeInfoContainer) + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodeInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodeInfoContainer) diff --git a/tesseract_task_composer/core/src/task_composer_node_ports.cpp b/tesseract_task_composer/core/src/task_composer_node_ports.cpp index 7f09419d5ef..17c21f3664b 100644 --- a/tesseract_task_composer/core/src/task_composer_node_ports.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_ports.cpp @@ -153,5 +153,5 @@ void TaskComposerNodePorts::serialize(Archive& ar, const unsigned int /*version* } } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodePorts) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerNodePorts) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerNodePorts) diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp index a8aa4cc42cc..d59494065ed 100644 --- a/tesseract_task_composer/core/src/task_composer_pipeline.cpp +++ b/tesseract_task_composer/core/src/task_composer_pipeline.cpp @@ -123,5 +123,5 @@ void TaskComposerPipeline::serialize(Archive& ar, const unsigned int /*version*/ } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerPipeline) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerPipeline) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerPipeline) diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp index 9dc2a13664e..ea080032283 100644 --- a/tesseract_task_composer/core/src/task_composer_task.cpp +++ b/tesseract_task_composer/core/src/task_composer_task.cpp @@ -74,5 +74,5 @@ void TaskComposerTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerTask) diff --git a/tesseract_task_composer/core/src/test_suite/test_task.cpp b/tesseract_task_composer/core/src/test_suite/test_task.cpp index d98f12b852d..f3fe7a718c1 100644 --- a/tesseract_task_composer/core/src/test_suite/test_task.cpp +++ b/tesseract_task_composer/core/src/test_suite/test_task.cpp @@ -128,5 +128,5 @@ std::unique_ptr TestTask::runImpl(TaskComposerContext& con } // namespace tesseract_planning::test_suite -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::test_suite::TestTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::test_suite::TestTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::test_suite::TestTask) diff --git a/tesseract_task_composer/examples/CMakeLists.txt b/tesseract_task_composer/examples/CMakeLists.txt index 2e92cb5fee8..1a62a5d94d2 100644 --- a/tesseract_task_composer/examples/CMakeLists.txt +++ b/tesseract_task_composer/examples/CMakeLists.txt @@ -1,7 +1,6 @@ find_package(tesseract_environment REQUIRED) find_package(tesseract_command_language REQUIRED) find_package(tesseract_visualization REQUIRED) -find_package(tesseract_support REQUIRED) find_package(Taskflow REQUIRED) add_executable(${PROJECT_NAME}_example task_composer_example.cpp) @@ -11,8 +10,7 @@ target_link_libraries( ${PROJECT_NAME}_taskflow console_bridge::console_bridge tesseract::tesseract_environment - tesseract::tesseract_command_language - tesseract::tesseract_support) + tesseract::tesseract_command_language) target_compile_options(${PROJECT_NAME}_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_example PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -29,8 +27,7 @@ target_link_libraries( console_bridge::console_bridge tesseract::tesseract_environment tesseract::tesseract_command_language - tesseract::tesseract_visualization - tesseract::tesseract_support) + tesseract::tesseract_visualization) target_compile_options(${PROJECT_NAME}_trajopt_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_trajopt_example PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) @@ -47,8 +44,7 @@ target_link_libraries( console_bridge::console_bridge tesseract::tesseract_environment tesseract::tesseract_command_language - tesseract::tesseract_visualization - tesseract::tesseract_support) + tesseract::tesseract_visualization) target_compile_options(${PROJECT_NAME}_raster_example PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME}_raster_example PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index b159e6bed59..bb3a124f152 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -22,7 +22,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; @@ -31,10 +31,12 @@ int main() // -------------------- // Perform setup // -------------------- - auto locator = std::make_shared(); + auto locator = std::make_shared(); auto env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); env->init(urdf_path, srdf_path, locator); // Dynamically load ignition visualizer if exist diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp index ade959accad..fccab3a9da2 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -23,7 +23,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; @@ -32,10 +32,12 @@ int main() // -------------------- // Perform setup // -------------------- - auto locator = std::make_shared(); + auto locator = std::make_shared(); tesseract_environment::Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); env->init(urdf_path, srdf_path, locator); // Dynamically load ignition visualizer if exist diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index ad2735ce02c..909a496825d 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -202,5 +202,5 @@ void ContinuousContactCheckTask::serialize(Archive& ar, const unsigned int /*ver } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ContinuousContactCheckTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ContinuousContactCheckTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ContinuousContactCheckTask) diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index 9f071b3d848..99c4f7d558f 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -197,5 +197,5 @@ void DiscreteContactCheckTask::serialize(Archive& ar, const unsigned int /*versi } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DiscreteContactCheckTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DiscreteContactCheckTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DiscreteContactCheckTask) diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp index 8061666ce78..3202ee2cef4 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp @@ -286,5 +286,5 @@ void FixStateBoundsTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateBoundsTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateBoundsTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateBoundsTask) diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 8eb28c4874a..edbb07b212e 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -777,5 +777,5 @@ void FixStateCollisionTask::serialize(Archive& ar, const unsigned int /*version* } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateCollisionTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateCollisionTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateCollisionTask) diff --git a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp index 73ac2197d11..97fb51e6086 100644 --- a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp @@ -179,5 +179,5 @@ void FormatAsInputTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsInputTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FormatAsInputTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsInputTask) diff --git a/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp b/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp index bf263966b2e..65282f1cd7b 100644 --- a/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp @@ -106,5 +106,5 @@ void FormatAsResultTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsResultTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FormatAsResultTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsResultTask) diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index a46d4f97dcd..936d983e21f 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -262,5 +262,5 @@ void IterativeSplineParameterizationTask::serialize(Archive& ar, const unsigned } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::IterativeSplineParameterizationTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::IterativeSplineParameterizationTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::IterativeSplineParameterizationTask) diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 81d6870c506..5fe97069635 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -201,5 +201,5 @@ void MinLengthTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MinLengthTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MinLengthTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MinLengthTask) diff --git a/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp index 7c9027e1487..746d6f68b09 100644 --- a/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp @@ -116,5 +116,5 @@ void ProcessPlanningInputTask::serialize(Archive& ar, const unsigned int /*versi } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProcessPlanningInputTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProcessPlanningInputTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProcessPlanningInputTask) diff --git a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp index c1c004834c0..a74dac807d9 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -128,5 +128,5 @@ void ProfileSwitchTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProfileSwitchTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProfileSwitchTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProfileSwitchTask) diff --git a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp index b07d272f236..8c690714844 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -614,5 +614,5 @@ void RasterMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RasterMotionTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RasterMotionTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RasterMotionTask) diff --git a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp index da4631cb927..4bf433363c8 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp @@ -502,5 +502,5 @@ void RasterOnlyMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RasterOnlyMotionTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RasterOnlyMotionTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RasterOnlyMotionTask) diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp index c8195d4760c..19f118ef917 100644 --- a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -248,5 +248,5 @@ void RuckigTrajectorySmoothingTask::serialize(Archive& ar, const unsigned int /* } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingTask) diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp index 7696e6f7f1b..3704292f0b9 100644 --- a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp @@ -231,5 +231,5 @@ void TimeOptimalParameterizationTask::serialize(Archive& ar, const unsigned int } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TimeOptimalParameterizationTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TimeOptimalParameterizationTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TimeOptimalParameterizationTask) diff --git a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp index b0860173ef0..a1608c5dffc 100644 --- a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp @@ -150,5 +150,5 @@ void UpdateEndStateTask::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateEndStateTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpdateEndStateTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateEndStateTask) diff --git a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp index f6a98dfb461..f34a613cd27 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp @@ -181,5 +181,5 @@ void UpdateStartAndEndStateTask::serialize(Archive& ar, const unsigned int /*ver } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateStartAndEndStateTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpdateStartAndEndStateTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateStartAndEndStateTask) diff --git a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp index 4ac75bc5960..0c40487c002 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp @@ -153,5 +153,5 @@ void UpdateStartStateTask::serialize(Archive& ar, const unsigned int /*version*/ } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateStartStateTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpdateStartStateTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpdateStartStateTask) diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp index 805b5637832..927cfd6abc8 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -210,5 +210,5 @@ void UpsampleTrajectoryTask::serialize(Archive& ar, const unsigned int /*version } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpsampleTrajectoryTask) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpsampleTrajectoryTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpsampleTrajectoryTask) diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp index e5398b11e25..c88cc8b8432 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -251,5 +251,5 @@ void TaskflowTaskComposerExecutor::serialize(Archive& ar, const unsigned int ver } // namespace tesseract_planning -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskflowTaskComposerExecutor) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskflowTaskComposerExecutor) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskflowTaskComposerExecutor) diff --git a/tesseract_task_composer/test/CMakeLists.txt b/tesseract_task_composer/test/CMakeLists.txt index ac78c3a9838..dbcec749e69 100644 --- a/tesseract_task_composer/test/CMakeLists.txt +++ b/tesseract_task_composer/test/CMakeLists.txt @@ -1,4 +1,3 @@ -find_package(tesseract_support REQUIRED) find_package( tesseract_motion_planners REQUIRED COMPONENTS core simple @@ -20,7 +19,6 @@ target_link_libraries( ${PROJECT_NAME}_fix_state_bounds_task_unit PRIVATE GTest::GTest GTest::Main - tesseract::tesseract_support ${PROJECT_NAME}_planning_nodes ${TESSERACT_TCMALLOC_LIB}) target_include_directories(${PROJECT_NAME}_fix_state_bounds_task_unit @@ -42,7 +40,6 @@ target_link_libraries( ${PROJECT_NAME}_fix_state_collision_task_unit PRIVATE GTest::GTest GTest::Main - tesseract::tesseract_support ${PROJECT_NAME}_planning_nodes ${TESSERACT_TCMALLOC_LIB}) target_include_directories(${PROJECT_NAME}_fix_state_collision_task_unit diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 94ef2c3ef16..02c31f81bd6 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -16,7 +16,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include using namespace tesseract_planning; using namespace tesseract_environment; @@ -32,11 +32,13 @@ class FixStateBoundsTaskUnit : public ::testing::Test void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp index 3ae1084ead0..be3c990f64e 100644 --- a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp @@ -13,7 +13,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include @@ -30,11 +30,13 @@ class FixStateCollisionTaskUnit : public ::testing::Test void SetUp() override { - auto locator = std::make_shared(); + auto locator = std::make_shared(); Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/boxbot.srdf"); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/boxbot.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/boxbot.srdf")->getFilePath()); EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); env_ = env; diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp index 58ef56d2b33..e4dd7d2411e 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -16,6 +16,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -140,6 +141,16 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerContextTests) // NOLINT test_suite::runSerializationPointerTest(context, "TaskComposerContextTests"); } +TEST(TesseractTaskComposerCoreUnit, TaskComposerLogTests) // NOLINT +{ + tesseract_planning::TaskComposerLog log; + log.context = + std::make_unique("TaskComposerLogTests", std::make_unique()); + + // Serialization + test_suite::runSerializationTest(log, "TaskComposerLogTests"); +} + TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLINT { test_suite::DummyTaskComposerNode node; diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 4e547b2c137..3f8ddc091c1 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -2552,25 +2553,34 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / data->setData("input_data", context->data_storage->getData("output_data")); EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); } + tesseract_planning::TaskComposerLog log; auto profiles = std::make_shared(); data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); - auto context = std::make_unique("abc", std::move(data)); + log.initial_data = *data; + + log.context = std::make_shared("abc", std::move(data)); MotionPlannerTask task( "abc", "input_data", "environment", "profiles", "output_data", false, true); - EXPECT_EQ(task.run(*context), 1); - auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*log.context), 1); + auto node_info = log.context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->status_code, 1); EXPECT_EQ(node_info->status_message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(context->isAborted(), false); - EXPECT_EQ(context->isSuccessful(), true); - EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); - EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(log.context->isAborted(), false); + EXPECT_EQ(log.context->isSuccessful(), true); + EXPECT_GE(log.context->data_storage->getData("output_data").as().size(), 10); + EXPECT_TRUE(log.context->task_infos.getAbortingNode().is_nil()); test_suite::runSerializationTest(*node_info, "TaskComposerMotionPlannerNodeInfoTests"); + { + const std::string filepath = tesseract_common::getTempPath() + "TaskComposerMotionPlannerLogTests.xml"; + tesseract_common::Serialization::toArchiveFileXML(log, filepath); + auto ninput = tesseract_common::Serialization::fromArchiveFileXML(filepath); + EXPECT_TRUE(ninput.initial_data.hasKey("environment")); + } } { // Failure missing input data