Skip to content
Snippets Groups Projects
Commit 00ae3dc7 authored by Tommy Persson's avatar Tommy Persson
Browse files

Work on ROS2

parent ee7737ff
No related branches found
No related tags found
No related merge requests found
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment