Fix std::function Python field assignment typemap

tesseract-robotics · Dec 3, 2023 · bf6da0e · bf6da0e
1 parent 2c8b9f6
commit bf6da0e
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 1 deletion.
diff --git a/tesseract_python/swig/tesseract_std_function.i b/tesseract_python/swig/tesseract_std_function.i
@@ -83,6 +83,23 @@ public:
   }
 }
 
+%typemap(in) Namespace::Name * (void *argp, int res = 0, std::shared_ptr< Name##Base > temp1, Namespace::Name temp2) {
+    // const tesseract_std_function& %typemap(in)
+  int newmem = 0;
+  res = SWIG_ConvertPtrAndOwn($input, &argp, $descriptor(std::shared_ptr< Name##Base > *), %convertptr_flags, &newmem);
+  if (!SWIG_IsOK(res)) {
+    %argument_fail(res, "$type", $symname, $argnum);
+  }
+  if (!argp) {
+    %argument_nullref("$type", $symname, $argnum);
+  } else {
+    temp1 = *(%reinterpret_cast(argp, std::shared_ptr< Name##Base > *));
+    temp2 = [temp1]( %formacro_2n(_tesseract_std_function_call_args,__VA_ARGS__) ) { return temp1->call(  %formacro_2n(_tesseract_std_function_call_vars,__VA_ARGS__)  ); };
+    $1 = &temp2;
+    if (newmem & SWIG_CAST_NEW_MEMORY) delete %reinterpret_cast(argp, std::shared_ptr< Name##Base > *);
+  }
+}
+
 %typemap(out) Namespace::Name const &  {
     // tesseract_std_function & %typemap(out)
     Py_INCREF(Py_None);

diff --git a/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py b/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py
@@ -15,7 +15,7 @@
 from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse
 from tesseract_robotics.tesseract_motion_planners_descartes import DescartesDefaultPlanProfileD, \
     DescartesMotionPlannerD, DescartesPlanProfileD, \
-    ProfileDictionary_addProfile_DescartesPlanProfileD, cast_DescartesPlanProfileD
+    ProfileDictionary_addProfile_DescartesPlanProfileD, cast_DescartesPlanProfileD, PoseSamplerFn
 from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram
 from ..tesseract_support_resource_locator import TesseractSupportResourceLocator
 
@@ -89,3 +89,14 @@ def test_descartes_freespace_fixed_poses():
         assert len(wp.getNames()) == 6
         assert isinstance(wp.getPosition(),np.ndarray)
         assert len(wp.getPosition()) == 6
+
+def _sampler_fun(tool_pose):
+    return []
+
+def test_sampler_std_fun():
+
+    plan_profile = DescartesDefaultPlanProfileD()
+    # DescartesDefaultPlanProfileD is not upcasting automatically, use helper function
+    plan_profile1 = cast_DescartesPlanProfileD(plan_profile)
+    pp = PoseSamplerFn(_sampler_fun)
+    plan_profile.target_pose_sampler = pp