From aadaea57933d4dc222c55aea523c932a640f7c05 Mon Sep 17 00:00:00 2001
From: Tommy Persson <@tommy.persson@liu.se>
Date: Fri, 11 Mar 2022 17:03:38 +0100
Subject: [PATCH] Work on ROS2
---
README.md | 2 +-
lrs_exec/exec_tree.py | 27 ++++-----------------------
2 files changed, 5 insertions(+), 24 deletions(-)
diff --git a/README.md b/README.md
index 1c1302e..130ad0a 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,3 @@
# lrs_exec
-For now pu all exec related stuff here (Python3).
\ No newline at end of file
+For now put all exec related stuff here (Python3).
\ No newline at end of file
diff --git a/lrs_exec/exec_tree.py b/lrs_exec/exec_tree.py
index 33b1b3b..756b213 100644
--- a/lrs_exec/exec_tree.py
+++ b/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")
--
GitLab