From bf6da0ec655a4fb23258ec3e8e21a307d143aa36 Mon Sep 17 00:00:00 2001
From: John Wason <wason@wasontech.com>
Date: Sat, 2 Dec 2023 22:37:04 -0500
Subject: [PATCH] Fix std::function Python field assignment typemap

---
 tesseract_python/swig/tesseract_std_function.i  | 17 +++++++++++++++++
 .../test_descartes_planner.py                   | 13 ++++++++++++-
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/tesseract_python/swig/tesseract_std_function.i b/tesseract_python/swig/tesseract_std_function.i
index daba22ff0..8c118ab05 100644
--- 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
index 61b979b49..7957628c7 100644
--- 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
\ No newline at end of file