Work on ROS2

lrs_exec/exec_tree.py

@@ -10,7 +10,7 @@ from lrs_srvs_wdb.srv import WDBWorldToGeo
 from lrs_srvs_tst.srv import TSTCreateTree, TSTDistributeTreeFromUuid, TSTSetStartExecutorFlagFromUuid
 from lrs_msgs_common.srv import GetResponse
 from lrs_msgs_common.msg import DisplayRequest
-from lrs_srvs_tst.srv import TSTExpandTree
+from lrs_srvs_tst.srv import TSTExpandTree, TSTAddConstraintsToTree
 
 class ExecTree(Node):
 
@@ -18,6 +18,7 @@ class ExecTree(Node):
         super().__init__(node_name)
         self.group = ReentrantCallbackGroup()
 
+        self.add_constraints_client = self.create_client(TSTAddConstraintsToTree, "tst_add_constraints_to_tree", callback_group=self.group)
         self.expand_tree_client = self.create_client(TSTExpandTree, "tst_expand_tree", callback_group=self.group)
         self.world_to_geo_client = self.create_client(WDBWorldToGeo, "wdb/world_to_geo", callback_group=self.group)
         self.create_tree_client = self.create_client(TSTCreateTree, "exec/tstfactory/create_tree", callback_group=self.group)
@@ -62,6 +63,18 @@ class ExecTree(Node):
         except Exception as exc:
             self.get_logger().error(f'EXCEPTION expand_tree: {type(exc)} - {exc}')
     
+    def add_constraints(self, jobj):
+        jobjstr = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
+        print("add_constraints:", jobjstr)
+        try:
+            req = TSTAddConstraintsToTree.Request()
+            req.input_json = jobjstr
+            future = self.add_constraints_client.call_async(req)
+            resp = self.wait_future(future, print_waiting=True)
+            return resp
+        except Exception as exc:
+            self.get_logger().error(f'EXCEPTION add_constraints: {type(exc)} - {exc}')
+    
     def world_to_geo(self, x, y, z):
         print("world_to_geo:", x, y, z)
         try:
@@ -178,7 +191,7 @@ class ExecTree(Node):
         return (False, "Timeout")                
         
 
-    def handle_tree(self, jobj, exectree=True, displaytype="exec"):
+    def handle_tree(self, jobj, exectree=True, add_constraints=False, displaytype="exec"):
         try:
 
             # expand tree
@@ -186,12 +199,14 @@ class ExecTree(Node):
             if resp and resp.success:
                 jobj = json.loads(resp.expanded_json)
 
-            # get constraints
-
-            # or do it in exoand tree...
+            if add_constraints:
+                # get constraints
+                resp = self.add_constraints(jobj)
+                if resp and resp.success:
+                    jobj = json.loads(resp.output_json)
             
             jobjstr = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
-            # print("JOBJSTR:", jobjstr)
+            print("JOBJSTR:", jobjstr)
             
             resp = self.create_tree(jobjstr)
             # print("create_tree RESP:", resp)

lrs_exec/executor_factory.py

@@ -10,10 +10,11 @@ from lrs_exec.parameters import Parameters
 
 # Needed for Task Execution Level and TST Execution Level
 from lrs_srvs_exec.srv import TSTCreateExecutor, TSTDestroyExecutor, TSTSignalExecutor, TSTStartExecutor
+from lrs_srvs_exec.srv import TSTExecutorJsonExpand, TSTExecutorJsonConstraints
 from lrs_srvs_tst.srv import TSTGetNode
 
 # Needed for Delegation Level
-from lrs_srvs_exec.srv import TSTExecutorCheckFromTni, TSTExecutorExpand, TSTExecutorJsonExpand
+from lrs_srvs_exec.srv import TSTExecutorCheckFromTni, TSTExecutorExpand
 from lrs_srvs_exec.srv import TSTExecutorIsDelegationExpandable, TSTExecutorGetConstraintsFromTni
 from lrs_srvs_wdb.srv import WDBAddCapability
 
@@ -56,6 +57,7 @@ class ExecutorFactory(Node):
         self.add_serv(TSTStartExecutor, "start_executor", self.start_executor)  
         self.add_serv(TSTSignalExecutor, "signal_executor", self.signal_executor)
         self.add_serv(TSTExecutorJsonExpand, "executor_json_expand", self.executor_json_expand)
+        self.add_serv(TSTExecutorJsonConstraints, "executor_json_constraints", self.executor_json_constraints)
         
         # Task Execution Level clients
         self.get_node_client = self.create_client(TSTGetNode, "exec/tstfactory/get_node", callback_group=self.group)
@@ -251,6 +253,34 @@ class ExecutorFactory(Node):
         self.lprint("executor check:", response)            
         return response
 
+    def json_constraints(self, node_json):
+        res = ""
+        try:
+            jobj = json.loads(node_json)
+            if "name" in jobj:
+                name = jobj["name"].replace("-", "_")
+        except Exception as exc:
+            print("EXCEPTION:", type(exc))
+            print(exc)
+        return res
+
+    def executor_json_constraints(self, request, response):
+        try:
+            self.lprint("executor constraints:", request)
+            response.constraints_json = self.json_constraints(request.node_json)
+            response.success = True
+            response.error = 0
+            response.reason = ""
+        except Exception as exc:
+            print(type(exc))
+            print(exc)
+            response.free_id = 0
+            response.success = False
+            response.error = 2
+            response.reason = f'executor_json_constraints throw exception'
+        self.lprint("executor_json_constraints response:", response)                    
+        return response
+    
     def json_expand(self, node_json):
         res = ""
         try:

lrs_exec/tst_command.py

@@ -34,6 +34,7 @@ class TstCommand(ExecTree):
         self.declare_parameter("area_type", "forrest")
         self.declare_parameter("vehicle", "tbot")                
         self.declare_parameter("op0", False)                
+        self.declare_parameter("unit", "/turtle0")                
 
         self.vehicle = self.get_parameter("vehicle").value
         self.command = self.get_parameter("command").value
@@ -47,6 +48,7 @@ class TstCommand(ExecTree):
         self.target_size = self.get_parameter("target_size").value
         self.area_type = self.get_parameter("area_type").value
         self.op0 = self.get_parameter("op0").value
+        self.unit = self.get_parameter("unit").value
         
         #self.cli = self.create_client(AddTwoInts, 'add_two_ints')
         #while not self.cli.wait_for_service(timeout_sec=1.0):
@@ -54,6 +56,7 @@ class TstCommand(ExecTree):
         #self.req = AddTwoInts.Request()
         self.ns = self.get_namespace()
         print("NAMESPACE:", self.ns)
+        print("UNIT:", self.unit)
 
         self.root_ns = self.ns
         if self.op0:
@@ -76,10 +79,10 @@ class TstCommand(ExecTree):
     def move(self):
         try:
             (lat, lon, alt) = self.xyz_to_latlonalt(self.x, self.y, self.z)
-            moveto = json_basic_moveto(self.ns, lat, lon, alt, speed=self.sspeed)
+            moveto = json_basic_moveto(self.unit, lat, lon, alt, speed=self.sspeed)
             tree = json_basic_seq(self.root_ns)
             tree["children"].append(moveto)
-            (res, reason) = self.handle_tree(tree)
+            (res, reason) = self.handle_tree(tree, add_constraints=True)
             print(res, reason)
         except Exception as exc:
             print("move", type(exc))

lrs_exec/tst_constraint.py

@@ -13,7 +13,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup
 from lrs_msgs_tst.msg import TSTConstraintCommand, TSTConstraintResponse
 from lrs_srvs_tst.srv import TSTAddConstraintsToTree
 
-from lrs_srvs_exec.srv import TSTExecutorJsonExpand
+from lrs_srvs_exec.srv import TSTExecutorJsonConstraints
 from lrs_exec.expand import get_exec_unit
 from lrs_msgs_common.msg import UnitInfoSlow
 
@@ -33,9 +33,7 @@ class ConstraintNode(Node):
         
         self.com_uuid_to_jobj = {}
         self.trees = {}
-        self.expand_clients = {}
-        self.expandable_types = None
-        self.have_expandable_types = False
+        self.constraints_clients = {}
 
     def wait_future(self, future, print_waiting=False):
         try:
@@ -55,6 +53,7 @@ class ConstraintNode(Node):
                     self.do_add_constraints(c)
             else:
                 ns = get_exec_unit(jobj)
+                name = jobj["name"].replace("-", "_")                
                 if ns:
                     msg = TSTConstraintCommand()
                     com_uuid = str(uuid.uuid4())
@@ -62,6 +61,7 @@ class ConstraintNode(Node):
                     jobj["constraints_added"] = False
                     msg.com_uuid = com_uuid
                     msg.ns = ns
+                    msg.type = name
                     msg.json = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
                     self.constraint_command_pub.publish(msg)
         except Exception as exc:
@@ -94,13 +94,13 @@ class ConstraintNode(Node):
 
     def add_constraints(self, request, response):
         try:
-            print("add_constraints:", request)
+            # print("add_constraints:", request)
             jobj = json.loads(request.input_json)
             #tree_uuid = str(uuid.uuid4())
             #self.trees[tree_uuid] = jobj
 
             self.do_add_constraints(jobj)
-            while not self.is_expanded(jobj):
+            while not self.is_finished(jobj):
                 # print("wait for expansion")
                 #print(json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': ')))
                 time.sleep(0.1)
@@ -110,7 +110,7 @@ class ConstraintNode(Node):
             response.success = True
         except Exception as exc:
             self.get_logger().error(f'EXCEPTION add_constraints: {type(exc)} - {exc}')
-            response.expanded_json = ""
+            response.output_json = ""
             response.success = False
             response.error = 1
             response.reason = str(exc)
@@ -119,33 +119,31 @@ class ConstraintNode(Node):
     def constraint_command_callback(self, msg):        
         try:
             if msg.ns == self.get_namespace():
-                # print("expand_command_callback:", msg)
+                print("constraint_command_callback:", msg)
                 # jobj = json.loads(msg.json)
                 # print(json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': ')))
 
                 # Later maybe add optimisation similar to expand
 
                 resp = None
-                
-                ## self.expand_clients[type] = self.create_client(TSTExecutorJsonExpand, f'tst_executor/{type}/executor_json_expand', callback_group=self.group)
-                # print("CLIENT:", self.expand_clients[type])
-                ## req = TSTExecutorJsonExpand.Request()
-                ## req.node_json = msg.json
-                # print("SERVICE:", f'tst_executor/{type}/executor_json_expand')
-                # print("REQ:", req)
-                ## future = self.expand_clients[type].call_async(req)
-                ## resp = self.wait_future(future)
-                # print("RESP:", resp)
-
-                jobj = {"speed": ["slow", "standard"]}
-                jobjstr = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
-                    
-                respmsg = TSTExpandResponse()
+
+                if msg.type:
+                    type = msg.type.replace("-","_")                    
+                    self.constraints_clients[type] = self.create_client(TSTExecutorJsonConstraints, f'tst_executor/{type}/executor_json_constraints', callback_group=self.group)
+                    # print("CLIENT:", self.expand_clients[type])
+                    req = TSTExecutorJsonConstraints.Request()
+                    req.node_json = msg.json
+                    # print("SERVICE:", f'tst_executor/{type}/executor_json_expand')
+                    # print("REQ:", req)
+                    future = self.constraints_clients[type].call_async(req)
+                    resp = self.wait_future(future)
+                    # print("RESP:", resp)
+
+                respmsg = TSTConstraintResponse()
                 respmsg.response_to = msg.com_uuid
                 respmsg.ns = msg.ns
-                if True or resp:
-                    #respmsg.json = resp.expansion_json
-                    respmsg.json = jobjstr
+                if resp:
+                    respmsg.json = resp.constraints_json
                 else:
                     respmsg.json = ""
                 self.constraint_response_pub.publish(respmsg)