Work on ROS2

README.md

 # lrs_exec
 
-For now pu all exec related stuff here (Python3).
+For now put all exec related stuff here (Python3).
\ No newline at end of file

lrs_exec/exec_tree.py

@@ -12,7 +12,7 @@ from lrs_msgs_common.srv import GetResponse
 from lrs_msgs_common.msg import DisplayRequest
 from lrs_srvs_tst.srv import TSTExpandTree, TSTAddConstraintsToTree
 
-from lrs_util.constraints import Constraints, as_constraint
+from lrs_util.constraints import TreeConstraints
 
 class ExecTree(Node):
 
@@ -192,27 +192,6 @@ class ExecTree(Node):
         print("wait_for_response timeout:", waittime, resp)
         return (False, "Timeout")
 
-    def check_cons(self, params, jsonstr):
-        cons = json.loads(jsonstr, object_hook=as_constraint)
-        if "speed" in params:
-            print("check_cons setting speed:", params["speed"])
-            cons.set_speed(params["speed"])
-        return cons.solve()
-    
-    def check_constraints(self, jobj):
-        res = True
-        if "constraints" in jobj:
-            print("CHECK CONS")
-            subres = self.check_cons(jobj["params"], json.dumps(jobj["constraints"], indent = 4))
-            print("RES:", subres)
-            if not subres:
-                res = False
-            
-        for children in jobj["children"]:
-            flag = self.check_constraints(children)
-            res &= flag
-        return res
-
     def handle_tree(self, jobj, exectree=True, add_constraints=False, displaytype="exec"):
         try:
 
@@ -229,7 +208,9 @@ class ExecTree(Node):
 
                 # check constraints
 
-                res = self.check_constraints(jobj)
+                tc = TreeConstraints(jobj=jobj)
+                res = tc.check_constraints_isolated()
+                
                 if not res:
                     print("CONSTRAINT CHECK FAILED")
                     return (False, "CONSTRAINT CHECK FAILED")