From 00ae3dc73538b2e170eb56d75f45a72b9e7871a4 Mon Sep 17 00:00:00 2001
From: Tommy Persson <@tommy.persson@liu.se>
Date: Thu, 10 Mar 2022 17:14:54 +0100
Subject: [PATCH] Work on ROS2
---
lrs_exec/exec_tree.py | 33 +++++++++++++++++++++++++++++++--
1 file changed, 31 insertions(+), 2 deletions(-)
diff --git a/lrs_exec/exec_tree.py b/lrs_exec/exec_tree.py
index 3215b2b..33b1b3b 100644
--- a/lrs_exec/exec_tree.py
+++ b/lrs_exec/exec_tree.py
@@ -12,6 +12,8 @@ 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
+
class ExecTree(Node):
def __init__(self, node_name):
@@ -188,8 +190,28 @@ class ExecTree(Node):
print("Cannot get response")
return (False, "Cannot get response")
print("wait_for_response timeout:", waittime, resp)
- return (False, "Timeout")
-
+ 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:
@@ -204,6 +226,13 @@ class ExecTree(Node):
resp = self.add_constraints(jobj)
if resp and resp.success:
jobj = json.loads(resp.output_json)
+
+ # check constraints
+
+ res = self.check_constraints(jobj)
+ if not res:
+ print("CONSTRAINT CHECK FAILED")
+ return (False, "CONSTRAINT CHECK FAILED")
jobjstr = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
print("JOBJSTR:", jobjstr)
--
GitLab