[desig+fdplans] cleaner implementation for resetting designator fix

Author: gaya-

cram_actions/src/fetch-and-deliver-plans.lisp

@@ -203,7 +203,6 @@ retries with different search location or robot base location."
               (((or common-fail:navigation-goal-in-collision
                     common-fail:looking-high-level-failure
                     common-fail:perception-low-level-failure) (e)
-                 (setf ?robot-location (desig:copy-designator ?robot-location))
                  (common-fail:retry-with-loc-designator-solutions
                      ?robot-location
                      robot-location-retries
@@ -285,7 +284,6 @@ and using the grasp and arm specified in `pick-up-action' (if not NIL)."
                 common-fail:perception-low-level-failure
                 common-fail:object-unreachable
                 common-fail:manipulation-low-level-failure) (e)
-             (setf ?pick-up-robot-location (desig:copy-designator ?pick-up-robot-location))
              (common-fail:retry-with-loc-designator-solutions
                  ?pick-up-robot-location
                  relocation-for-ik-retries
@@ -436,7 +434,6 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat
               (((or common-fail:navigation-goal-in-collision
                     common-fail:object-undeliverable
                     common-fail:manipulation-low-level-failure) (e)
-                 (setf ?target-robot-location (desig:copy-designator ?target-robot-location))
                  (common-fail:retry-with-loc-designator-solutions
                      ?target-robot-location
                      relocation-for-ik-retries

cram_core/cram_designators/src/cram-designators/designator-protocol.lisp

@@ -126,9 +126,15 @@
   (:method ((solution-1 array) (solution-2 array))
     (equalp solution-1 solution-2)))
 
-(defgeneric reset (desig)
-  (:documentation "Resets the designator by deleting all associated solutions.
-Used for recalculating a designator when its dependent designator got updated."))
+(defun reset (desig)
+  "Resets the designator by deleting all associated solutions.
+Used for recalculating a designator when its dependent designator got updated.
+The actual implementation creates a copy of the designator, such that only
+the description is preserved. The returned designator is equated to the input one,
+in case one would want to get back to the old one through the equated designator chain."
+  (let ((desig-copy (copy-designator desig)))
+    (equate desig desig-copy)
+    (setf desig desig-copy)))
 
 (defvar *designator-pprint-description* t
   "If set to T, DESIGNATOR objects will be pretty printed with their description.")
@@ -232,10 +238,6 @@ class (derived from class DESIGNATOR), e.g. OBJECT-DESIGNATOR."
   "Allow asking current-desig also on NULL objects."
   NIL)
 
-(defmethod reset ((desig designator))
-  "Empties the data slot"
-  (setf (slot-value desig 'data) nil))
-
 (defun newest-effective-designator (desig)
   (labels ((find-effective-desig (desig)
              (cond ((not desig) nil)

cram_core/cram_designators/src/cram-designators/location-designator.lisp

@@ -209,10 +209,6 @@ either :ACCEPT, :REJECT, :MAYBE-REJECT or :UNKNOWN."
 (defmethod resolve-designator ((desig location-designator) (role t))
   (resolve-location-designator-through-generators-and-validators desig))
 
-(defmethod reset :after ((desig location-designator))
-  "Empties the current-solution slot"
-  (setf (slot-value desig 'current-solution) nil))
-
 (defun validate-location-designator-solution (designator solution)
   (declare (type location-designator designator))
   (labels ((validate (validation-functions designator solution

cram_interfaces/cram_location_costmap/src/designator-integration.lisp

@@ -45,11 +45,6 @@
                 (unless (is-var ?cm)
                   ?cm))))))
 
-(defmethod desig:reset ((desig location-designator))
-  "Deleted cached costmap associated with this designator"
-  (let ((first-designator (desig:first-desig desig)))
-    (remhash first-designator *costmap-cache*)))
-
 (defun get-cached-costmap-maxvalue (costmap)
   (or (gethash costmap *costmap-max-values)
       (setf (gethash costmap *costmap-max-values)