Skip to content

Commit 3f2f818

Browse files
johnwasonjohnwason
committed
Update tesseract_task_composer module
1 parent ff77b99 commit 3f2f818

File tree

6 files changed

+128
-27
lines changed

6 files changed

+128
-27
lines changed

‎tesseract_python/CMakeLists.txt

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,7 @@ tesseract_python_module(tesseract_motion_planners_trajopt_python SWIG_SRCS swig/
149149
tesseract_python_module(tesseract_motion_planners_ompl_python SWIG_SRCS swig/tesseract_motion_planners_ompl_python.i PACKAGE tesseract_motion_planners_ompl LIBS tesseract::tesseract_motion_planners_ompl )
150150
tesseract_python_module(tesseract_motion_planners_descartes_python SWIG_SRCS swig/tesseract_motion_planners_descartes_python.i PACKAGE tesseract_motion_planners_descartes LIBS tesseract::tesseract_motion_planners_descartes )
151151
tesseract_python_module(tesseract_time_parameterization_python SWIG_SRCS swig/tesseract_time_parameterization_python.i PACKAGE tesseract_time_parameterization LIBS tesseract::tesseract_time_parameterization_core tesseract::tesseract_time_parameterization_isp tesseract::tesseract_time_parameterization_totg tesseract::tesseract_time_parameterization_ruckig)
152-
# tesseract_python_module(tesseract_task_composer_python SWIG_SRCS swig/tesseract_task_composer_python.i PACKAGE tesseract_task_composer LIBS tesseract::tesseract_task_composer tesseract::tesseract_task_composer_nodes tesseract::tesseract_task_composer_taskflow ) # tesseract::tesseract_task_composer_factories tesseract::tesseract_task_composer_planning_factories tesseract::tesseract_task_composer_planning tesseract::tesseract_task_composer_taskflow_factories )
152+
tesseract_python_module(tesseract_task_composer_python SWIG_SRCS swig/tesseract_task_composer_python.i PACKAGE tesseract_task_composer LIBS tesseract::tesseract_task_composer tesseract::tesseract_task_composer_nodes tesseract::tesseract_task_composer_taskflow tesseract::tesseract_collision_core ) # tesseract::tesseract_task_composer_factories tesseract::tesseract_task_composer_planning_factories tesseract::tesseract_task_composer_planning tesseract::tesseract_task_composer_taskflow_factories )
153153

154154
set(TESSERACT_PYTHON_MODULE_TARGETS
155155
_tesseract_common_python
@@ -169,7 +169,7 @@ set(TESSERACT_PYTHON_MODULE_TARGETS
169169
_tesseract_motion_planners_ompl_python
170170
_tesseract_motion_planners_descartes_python
171171
_tesseract_time_parameterization_python
172-
# _tesseract_task_composer_python
172+
_tesseract_task_composer_python
173173
)
174174

175175
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/setup.py.in" "${CMAKE_CURRENT_BINARY_DIR}/python/setup.py" @ONLY)

‎tesseract_python/swig/tesseract_command_language_python.i

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -227,4 +227,6 @@ const tesseract_planning::TYPE as_const_ ## TYPE() {return $self->as<const tesse
227227
%template(ProfileDictionary_removeProfile_##NAME) tesseract_planning::ProfileDictionary_removeProfile<tesseract_planning::TYPE>;
228228
%enddef
229229

230-
%include "tesseract_command_language/constants.h"
230+
%include "tesseract_command_language/constants.h"
231+
232+
%tesseract_any_poly_type_shared_ptr(ProfileDictionary,tesseract_planning);

‎tesseract_python/swig/tesseract_environment_python.i

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -191,4 +191,6 @@
191191
{
192192
return dynamic_cast<const tesseract_environment::CommandAppliedEvent&>(a);
193193
}
194-
}
194+
}
195+
196+
%tesseract_any_poly_type_shared_ptr(Environment, tesseract_environment)

‎tesseract_python/swig/tesseract_task_composer_python.i

Lines changed: 49 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,29 @@
3838

3939
%{
4040

41+
// tesseract_common
42+
#include <tesseract_common/plugin_loader.h>
43+
44+
// tesseract_kinematics
45+
#include <tesseract_kinematics/core/joint_group.h>
46+
#include <tesseract_kinematics/core/kinematic_group.h>
47+
48+
// tesseract_environment
49+
#include <tesseract_environment/commands.h>
50+
#include <tesseract_environment/events.h>
51+
#include <tesseract_environment/environment.h>
52+
53+
// tesseract_collision
54+
#include <tesseract_collision/core/types.h>
55+
#include <tesseract_collision/core/discrete_contact_manager.h>
56+
#include <tesseract_collision/core/continuous_contact_manager.h>
57+
58+
// tesseract_command_language
59+
#include <tesseract_command_language/fwd.h>
60+
#include <tesseract_command_language/move_instruction.h>
61+
#include <tesseract_command_language/composite_instruction.h>
62+
#include <tesseract_command_language/profile_dictionary.h>
63+
4164
// tesseract_motion_planners_simple
4265
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
4366
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h>
@@ -84,16 +107,18 @@
84107
// tesseract_task_composer
85108
#include <tesseract_task_composer/core/task_composer_node_info.h>
86109
#include <tesseract_task_composer/core/task_composer_data_storage.h>
87-
#include <tesseract_task_composer/core/task_composer_problem.h>
110+
//#include <tesseract_task_composer/core/task_composer_problem.h>
88111
#include <tesseract_task_composer/core/task_composer_node.h>
89112
#include <tesseract_task_composer/core/task_composer_graph.h>
90113
#include <tesseract_task_composer/core/task_composer_future.h>
91114
#include <tesseract_task_composer/core/task_composer_task.h>
92115
#include <tesseract_task_composer/core/task_composer_executor.h>
93116
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>
94117
#include <tesseract_task_composer/core/task_composer_server.h>
118+
#include <tesseract_task_composer/core/task_composer_pipeline.h>
119+
#include <tesseract_task_composer/core/task_composer_context.h>
95120

96-
#include <tesseract_task_composer/planning/planning_task_composer_problem.h>
121+
// #include <tesseract_task_composer/planning/planning_task_composer_problem.h>
97122

98123
#include <tesseract_task_composer/taskflow/taskflow_task_composer_plugin_factories.h>
99124
#include <tesseract_task_composer/planning/planning_task_composer_plugin_factories.h>
@@ -104,7 +129,7 @@
104129

105130
// tesseract_task_composer profiles
106131

107-
#include <tesseract_task_composer/planning/profiles/check_input_profile.h>
132+
// #include <tesseract_task_composer/planning/profiles/check_input_profile.h>
108133
#include <tesseract_task_composer/planning/profiles/contact_check_profile.h>
109134
#include <tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h>
110135
#include <tesseract_task_composer/planning/profiles/fix_state_collision_profile.h>
@@ -116,7 +141,6 @@
116141
#include <tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h>
117142

118143

119-
120144
#include <tesseract_geometry/geometries.h>
121145
#include <tesseract_common/resource_locator.h>
122146
#include <tesseract_srdf/kinematics_information.h>
@@ -144,14 +168,22 @@
144168
%unique_ptr_as(source_class_type, tesseract_planning, dest_class_type, tesseract_planning)
145169
%enddef
146170

171+
%include "tesseract_task_composer/core/fwd.h"
172+
173+
// task_composer_keys
174+
%include "tesseract_task_composer/core/task_composer_keys.h"
175+
%template(get) tesseract_planning::TaskComposerKeys::get<std::string>;
176+
147177
// task_composer_node_info
148178

149179
%s_u_ptr(TaskComposerNodeInfo)
150180
%s_u_ptr(TaskComposerNodeInfoContainer)
151-
%s_u_ptr(TaskComposerProblem)
181+
// %s_u_ptr(TaskComposerProblem)
152182
// TODO: Handle maps containing unique_ptr
153183
// %template(MapUuidTaskComposerNodeInfoUPtr) std::map<boost::uuids::uuid, std::unique_ptr<tesseract_planning::TaskComposerNodeInfo> >;
154184
%ignore tesseract_planning::TaskComposerNodeInfoContainer::getInfoMap;
185+
%ignore tesseract_planning::TaskComposerNodeInfo::find;
186+
%ignore tesseract_planning::TaskComposerNodeInfoContainer::find;
155187
%include "tesseract_task_composer/core/task_composer_node_info.h"
156188

157189
// task_composer_data_storage
@@ -160,6 +192,8 @@
160192
// TODO: Handle tesseract_common::AnyPoly
161193
%include "tesseract_task_composer/core/task_composer_data_storage.h"
162194

195+
%unique_ptr_constructor(tesseract_planning::TaskComposerDataStorage, %arg(), %arg());
196+
163197
// task_composer_problem
164198

165199
// %include "tesseract_task_composer/core/task_composer_problem.h"
@@ -222,6 +256,8 @@ enum class future_status {
222256
%shared_ptr(tesseract_planning::TaskComposerNodeFactory)
223257
%shared_ptr(tesseract_planning::TaskComposerExecutorFactory)
224258

259+
%shared_ptr(tesseract_planning::TaskComposerPluginFactory)
260+
225261
%include "tesseract_task_composer/core/task_composer_plugin_factory.h"
226262

227263
// task_composer_server
@@ -263,9 +299,10 @@ enum class future_status {
263299
%arg(env, profiles, name));*/
264300

265301
// contact_check_profile
266-
%shared_ptr(tesseract_planning::ContactCheckProfile)
267-
%include "tesseract_task_composer/planning/profiles/contact_check_profile.h"
268-
%tesseract_command_language_add_profile_type(ContactCheckProfile);
302+
// TODO: Fix library linking
303+
// %shared_ptr(tesseract_planning::ContactCheckProfile)
304+
// %include "tesseract_task_composer/planning/profiles/contact_check_profile.h"
305+
// %tesseract_command_language_add_profile_type(ContactCheckProfile);
269306

270307
// fix_state_bounds_profile
271308
%shared_ptr(tesseract_planning::FixStateBoundsProfile)
@@ -310,9 +347,10 @@ enum class future_status {
310347
%tesseract_command_language_add_profile_type(UpsampleTrajectoryProfile);
311348

312349
%init %{
313-
tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerPlanningFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
314-
tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskflowFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
315-
tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskFactoryAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
350+
// TODO: fix anchors
351+
// tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerPlanningFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
352+
// tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskflowFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
353+
// tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskFactoryAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS");
316354

317355
%}
318356

‎tesseract_python/swig/tesseract_type_erasure_macros.i

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,20 @@
77
}
88
%enddef
99

10+
%define %tesseract_erasure_ctor_shared_ptr(class_type,class_namespace,inner_type,inner_namespace)
11+
%inline {
12+
class_namespace::class_type class_type ## _wrap_ ## inner_type (std::shared_ptr<inner_namespace::inner_type> inner_waypoint)
13+
{
14+
return class_namespace::class_type (inner_waypoint);
15+
}
16+
17+
class_namespace::class_type class_type ## _wrap_ ## inner_type ## Const (std::shared_ptr<const inner_namespace::inner_type> inner_waypoint)
18+
{
19+
return class_namespace::class_type (inner_waypoint);
20+
}
21+
}
22+
%enddef
23+
1024
%define %tesseract_erasure_ctor2(class_type,class_namespace,inner_type)
1125
%inline {
1226
class_namespace::class_type class_type ## _wrap_ ## inner_type (inner_type inner_waypoint)
@@ -26,6 +40,21 @@
2640
}
2741
%enddef
2842

43+
%define %tesseract_erasure_as_shared_ptr(source_class_type,source_class_namespace,dest_class_type,dest_class_namespace)
44+
45+
%inline {
46+
std::shared_ptr<dest_class_namespace::dest_class_type> source_class_type ## _as_ ## dest_class_type (source_class_namespace::source_class_type& self)
47+
{
48+
return self.as<std::shared_ptr<dest_class_namespace::dest_class_type>>();
49+
}
50+
51+
std::shared_ptr<const dest_class_namespace::dest_class_type> source_class_type ## _as_ ## dest_class_type ## Const (source_class_namespace::source_class_type& self)
52+
{
53+
return self.as<std::shared_ptr<const dest_class_namespace::dest_class_type>>();
54+
}
55+
}
56+
%enddef
57+
2958
%define %tesseract_erasure_as2(source_class_type,source_class_namespace,dest_class_type)
3059

3160
%inline {
@@ -45,6 +74,20 @@
4574
}
4675
%enddef
4776

77+
%define %tesseract_erasure_is_shared_ptr(source_class_type,source_class_namespace,dest_class_type,dest_class_namespace)
78+
%inline {
79+
bool source_class_type ## _is_ ## dest_class_type (const source_class_namespace::source_class_type& self)
80+
{
81+
return self.getType() == typeid(std::shared_ptr<dest_class_namespace::dest_class_type>);
82+
}
83+
84+
bool source_class_type ## _is_ ## dest_class_type ## Const (const source_class_namespace::source_class_type& self)
85+
{
86+
return self.getType() == typeid(std::shared_ptr<const dest_class_namespace::dest_class_type>);
87+
}
88+
}
89+
%enddef
90+
4891
%define %tesseract_erasure_is2(source_class_type,source_class_namespace,dest_class_type)
4992
%inline {
5093
bool source_class_type ## _is_ ## dest_class_type (const source_class_namespace::source_class_type& self)
@@ -60,6 +103,12 @@
60103
%tesseract_erasure_is(AnyPoly,tesseract_common,TYPE,NAMESPACE)
61104
%enddef
62105

106+
%define %tesseract_any_poly_type_shared_ptr(TYPE,NAMESPACE)
107+
%tesseract_erasure_ctor_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE)
108+
%tesseract_erasure_as_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE)
109+
%tesseract_erasure_is_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE)
110+
%enddef
111+
63112
%define %tesseract_any_poly_type2(TYPE)
64113
%tesseract_erasure_ctor2(AnyPoly,tesseract_common,TYPE)
65114
%tesseract_erasure_as2(AnyPoly,tesseract_common,TYPE)

‎tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py

Lines changed: 22 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
import numpy.testing as nptest
66

77
from tesseract_robotics.tesseract_common import ResourceLocator, SimpleLocatedResource
8-
from tesseract_robotics.tesseract_environment import Environment
8+
from tesseract_robotics.tesseract_environment import Environment, AnyPoly_wrap_EnvironmentConst
99
from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \
1010
ManipulatorInfo, AnyPoly, AnyPoly_wrap_double
1111
from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \
@@ -15,7 +15,8 @@
1515
AnyPoly_wrap_CompositeInstruction, DEFAULT_PROFILE_KEY, JointWaypoint, JointWaypointPoly, \
1616
InstructionPoly_as_MoveInstructionPoly, WaypointPoly_as_StateWaypointPoly, \
1717
MoveInstructionPoly_wrap_MoveInstruction, StateWaypointPoly_wrap_StateWaypoint, \
18-
CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint
18+
CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint, \
19+
AnyPoly_wrap_ProfileDictionary
1920

2021
# from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse, generateInterpolatedProgram
2122
# from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \
@@ -26,7 +27,7 @@
2627
# TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
2728
# ProfileDictionary_addProfile_TrajOptCompositeProfile
2829
from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, \
29-
TaskComposerDataStorage, TaskComposerContext, PlanningTaskComposerProblem
30+
TaskComposerDataStorage, TaskComposerContext, TaskComposerDataStorageUPtr
3031

3132
from ..tesseract_support_resource_locator import TesseractSupportResourceLocator
3233

@@ -90,22 +91,27 @@ def test_task_composer_trajopt_example():
9091
task = factory.createTaskComposerNode("TrajOptPipeline")
9192
print("trajopt task name: " + task.getName())
9293

93-
output_key = task.getOutputKeys()[0]
94+
output_key = task.getOutputKeys().get("program")
95+
input_key = task.getInputKeys().get("planning_input")
9496

9597
profiles = ProfileDictionary()
9698

9799
program = freespace_example_progam_iiwa(manip_info)
98100

99-
problem_input = AnyPoly_wrap_CompositeInstruction(program)
100-
101-
planning_task_problem = PlanningTaskComposerProblem(env, profiles)
102-
planning_task_problem.input = problem_input
101+
problem_anypoly = AnyPoly_wrap_CompositeInstruction(program)
102+
environment_anypoly = AnyPoly_wrap_EnvironmentConst(env)
103+
profiles_anypoly = AnyPoly_wrap_ProfileDictionary(profiles)
103104

105+
task_data = TaskComposerDataStorage()
106+
task_data.setData(input_key, problem_anypoly)
107+
task_data.setData("environment", environment_anypoly)
108+
task_data.setData("profiles", profiles_anypoly)
109+
104110
task_executor = factory.createTaskComposerExecutor("TaskflowExecutor")
105111

106112
output_program = None
107113
try:
108-
future = task_executor.run(task.get(), planning_task_problem)
114+
future = task_executor.run(task.get(), task_data)
109115
future.wait()
110116

111117
output_program = AnyPoly_as_CompositeInstruction(future.context.data_storage.getData(output_key))
@@ -124,11 +130,15 @@ def test_task_composer_trajopt_example():
124130

125131
print("Done")
126132
finally:
127-
128133
# Cleanup memory to prevent segfault on exit
129-
del planning_task_problem
130-
del output_program
134+
# del planning_task_problem
135+
# del output_program
136+
del task_data
137+
del problem_anypoly
138+
del environment_anypoly
139+
del profiles_anypoly
131140
del future
132141
del task_executor
133142
del task
143+
134144

0 commit comments

Comments
 (0)